{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import roboticstoolbox as rtb\n",
    "from IPython.core.display import SVG"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will load a model for a simple Y-shaped planar robot.  Check out the source code to see how the model was constructed using `ELink` and `ERobot` classes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "┌───┬────────┬────────┬───────┬────────────────┐\n",
      "│id │  link  │ parent │ joint │      ETS       │\n",
      "├───┼────────┼────────┼───────┼────────────────┤\n",
      "│ 0\u001b[0m │  link0\u001b[0m │      -\u001b[0m │      \u001b[0m │ Rz(q0)\u001b[0m         │\n",
      "│ 1\u001b[0m │  link1\u001b[0m │  link0\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q1)\u001b[0m │\n",
      "│ 2\u001b[0m │ link2a\u001b[0m │  link1\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q2)\u001b[0m │\n",
      "│ 3\u001b[0m │ link3a\u001b[0m │ link2a\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q3)\u001b[0m │\n",
      "│ 4\u001b[0m │ link2b\u001b[0m │  link1\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q4)\u001b[0m │\n",
      "│ 5\u001b[0m │ link3b\u001b[0m │ link2b\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q5)\u001b[0m │\n",
      "│ 6\u001b[0m │   \u001b[38;5;4m@eea\u001b[0m │ link3a\u001b[0m │  None\u001b[0m │ tz(1)\u001b[0m          │\n",
      "│ 7\u001b[0m │   \u001b[38;5;4m@eeb\u001b[0m │ link3b\u001b[0m │  None\u001b[0m │ tz(1)\u001b[0m          │\n",
      "└───┴────────┴────────┴───────┴────────────────┘\n",
      "\n",
      "┌─────┬─────┬─────┬──────┬─────┬──────┬─────┐\n",
      "│name │ q0  │ q1  │ q2   │ q3  │ q4   │ q5  │\n",
      "├─────┼─────┼─────┼──────┼─────┼──────┼─────┤\n",
      "│  qz\u001b[0m │  0°\u001b[0m │  0°\u001b[0m │  0°\u001b[0m  │  0°\u001b[0m │  0°\u001b[0m  │  0°\u001b[0m │\n",
      "│  qy\u001b[0m │  0°\u001b[0m │  0°\u001b[0m │  45°\u001b[0m │  0°\u001b[0m │ -45°\u001b[0m │  0°\u001b[0m │\n",
      "└─────┴─────┴─────┴──────┴─────┴──────┴─────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "robot = rtb.models.ETS.Planar_Y()\n",
    "print(robot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From the displayed table we see that this robot has two end-effectors, indicated by the links shown in blue and prefixed with the @ symbol.\n",
    "\n",
    "We can easily create a graphical representation of the  turn the robot description into a [GraphViz dotfile](https://graphviz.org)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.dotfile('planar_y.dot')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which writes a file in the current folder.  We then render it to SVG format and [load and display it](https://ipython.readthedocs.io/en/stable/api/generated/IPython.display.html?highlight=svg#IPython.display.SVG)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\u001b]2;C02D8125PN78:/Users/corkep/code/robotics-toolbox-python/notebooks\u0007\u001b]1;C02D8125PN78\u0007"
     ]
    },
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"158pt\" viewBox=\"0.00 0.00 840.47 157.64\" width=\"840pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\"><g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 153.64)\"><title>G</title><polygon fill=\"white\" points=\"-4,4 -4,-153.64 836.47,-153.64 836.47,4 -4,4\" stroke=\"transparent\"/><!-- BASE --><g class=\"node\" id=\"node1\"><title>BASE</title><polygon fill=\"gray\" points=\"51,-100.65 0,-100.65 0,-49.65 51,-49.65 51,-100.65\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"25.5\" y=\"-71.45\">BASE</text></g><!-- link0 --><g class=\"node\" id=\"node2\"><title>link0</title><ellipse cx=\"153.6\" cy=\"-75.15\" fill=\"none\" rx=\"28.7\" ry=\"28.7\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"153.6\" y=\"-71.45\">link0</text></g><!-- BASE&#45;&gt;link0 --><g class=\"edge\" id=\"edge1\"><title>BASE-&gt;link0</title><path d=\"M51.11,-75.15C69.26,-75.15 94.28,-75.15 114.94,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"114.98,-78.65 124.98,-75.15 114.98,-71.65 114.98,-78.65\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"88\" y=\"-78.95\">Rz(q0)</text></g><!-- link1 --><g class=\"node\" id=\"node3\"><title>link1</title><ellipse cx=\"325.79\" cy=\"-75.15\" fill=\"none\" rx=\"28.7\" ry=\"28.7\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"325.79\" y=\"-71.45\">link1</text></g><!-- link0&#45;&gt;link1 --><g class=\"edge\" id=\"edge2\"><title>link0-&gt;link1</title><path d=\"M182.32,-75.15C210.76,-75.15 255.19,-75.15 286.87,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"287.08,-78.65 297.08,-75.15 287.08,-71.65 287.08,-78.65\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"239.69\" y=\"-78.95\">tx(1) * Rz(q1)</text></g><!-- link2a --><g class=\"node\" id=\"node4\"><title>link2a</title><ellipse cx=\"502.54\" cy=\"-117.15\" fill=\"none\" rx=\"32.49\" ry=\"32.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"502.54\" y=\"-113.45\">link2a</text></g><!-- link1&#45;&gt;link2a --><g class=\"edge\" id=\"edge3\"><title>link1-&gt;link2a</title><path d=\"M353.71,-81.62C382.34,-88.5 427.96,-99.46 460.96,-107.39\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"460.22,-110.81 470.76,-109.75 461.85,-104.01 460.22,-110.81\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"411.89\" y=\"-107.95\">tx(1) * Rz(q2)</text></g><!-- link2b --><g class=\"node\" id=\"node6\"><title>link2b</title><ellipse cx=\"502.54\" cy=\"-33.15\" fill=\"none\" rx=\"33.29\" ry=\"33.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"502.54\" y=\"-29.45\">link2b</text></g><!-- link1&#45;&gt;link2b --><g class=\"edge\" id=\"edge5\"><title>link1-&gt;link2b</title><path d=\"M353.71,-68.68C382.17,-61.84 427.41,-50.96 460.35,-43.05\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"461.25,-46.43 470.15,-40.69 459.61,-39.62 461.25,-46.43\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"411.89\" y=\"-67.95\">tx(1) * Rz(q4)</text></g><!-- link3a --><g class=\"node\" id=\"node5\"><title>link3a</title><ellipse cx=\"683.83\" cy=\"-117.15\" fill=\"none\" rx=\"32.49\" ry=\"32.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"683.83\" y=\"-113.45\">link3a</text></g><!-- link2a&#45;&gt;link3a --><g class=\"edge\" id=\"edge4\"><title>link2a-&gt;link3a</title><path d=\"M535.18,-117.15C564.66,-117.15 608.81,-117.15 641.15,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"641.2,-120.65 651.2,-117.15 641.2,-113.65 641.2,-120.65\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"593.18\" y=\"-120.95\">tx(1) * Rz(q3)</text></g><!-- eea --><g class=\"node\" id=\"node8\"><title>eea</title><ellipse cx=\"805.73\" cy=\"-117.15\" fill=\"none\" rx=\"22.96\" ry=\"22.96\" stroke=\"blue\"/><ellipse cx=\"805.73\" cy=\"-117.15\" fill=\"none\" rx=\"27\" ry=\"27\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"805.73\" y=\"-113.45\">eea</text></g><!-- link3a&#45;&gt;eea --><g class=\"edge\" id=\"edge7\"><title>link3a-&gt;eea</title><path d=\"M716.33,-117.15C732.27,-117.15 751.7,-117.15 768.27,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"768.69,-120.65 778.69,-117.15 768.69,-113.65 768.69,-120.65\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"747.98\" y=\"-120.95\">tz(1)</text></g><!-- link3b --><g class=\"node\" id=\"node7\"><title>link3b</title><ellipse cx=\"683.83\" cy=\"-33.15\" fill=\"none\" rx=\"33.29\" ry=\"33.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"683.83\" y=\"-29.45\">link3b</text></g><!-- link2b&#45;&gt;link3b --><g class=\"edge\" id=\"edge6\"><title>link2b-&gt;link3b</title><path d=\"M536.01,-33.15C565.2,-33.15 608.28,-33.15 640.23,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"640.6,-36.65 650.6,-33.15 640.6,-29.65 640.6,-36.65\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"593.18\" y=\"-36.95\">tx(1) * Rz(q5)</text></g><!-- eeb --><g class=\"node\" id=\"node9\"><title>eeb</title><ellipse cx=\"805.73\" cy=\"-33.15\" fill=\"none\" rx=\"22.96\" ry=\"22.96\" stroke=\"blue\"/><ellipse cx=\"805.73\" cy=\"-33.15\" fill=\"none\" rx=\"27\" ry=\"27\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"805.73\" y=\"-29.45\">eeb</text></g><!-- link3b&#45;&gt;eeb --><g class=\"edge\" id=\"edge8\"><title>link3b-&gt;eeb</title><path d=\"M717.28,-33.15C733.09,-33.15 752.17,-33.15 768.46,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"768.7,-36.65 778.7,-33.15 768.7,-29.65 768.7,-36.65\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"747.98\" y=\"-36.95\">tz(1)</text></g></g></svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "!dot -Tsvg planar_y.dot -x > planar_y.svg\n",
    "SVG(filename='planar_y.svg')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The fixed base of the robot is shown as the dark grey node.  Link reference frames are shown by circles and the edges are marked with the elementary transform sequences that define the relative poses of the link frames.  End-effector nodes are marked by a blue double circle.\n",
    "\n",
    "The ETS transforms in black indicate the joints, and they contain the five joint angles `q0` to `q5`.  The assignement of joint numbers is arbitrary and decided by the creator of the robot model.  All operations such as forward kinematics or Jacobians that accept a joint angle vector, organize it as `[q0, q1, q2, q3, q4 q5]`.\n",
    "\n",
    "Fixed joints  are shown in blue. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The forward kinematics is quite straightforward, but we must specify which end-effector we are interested in"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "SE3:  \u001b[38;5;1m 1          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;4m 3          \u001b[0m  \u001b[0m\n",
       "      \u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 1          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;4m 0          \u001b[0m  \u001b[0m\n",
       "      \u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 1          \u001b[0m\u001b[38;5;4m 1          \u001b[0m  \u001b[0m\n",
       "      \u001b[38;5;244m 0          \u001b[0m\u001b[38;5;244m 0          \u001b[0m\u001b[38;5;244m 0          \u001b[0m\u001b[38;5;244m 1          \u001b[0m  \u001b[0m\n",
       "    "
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.fkine(robot.qz, endlink='eea')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which is exactly the same as the pose of the second branch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "SE3:  \u001b[38;5;1m 1          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;4m 3          \u001b[0m  \u001b[0m\n",
       "      \u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 1          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;4m 0          \u001b[0m  \u001b[0m\n",
       "      \u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 0          \u001b[0m\u001b[38;5;1m 1          \u001b[0m\u001b[38;5;4m 1          \u001b[0m  \u001b[0m\n",
       "      \u001b[38;5;244m 0          \u001b[0m\u001b[38;5;244m 0          \u001b[0m\u001b[38;5;244m 0          \u001b[0m\u001b[38;5;244m 1          \u001b[0m  \u001b[0m\n",
       "    "
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.fkine(robot.qz, endlink='eeb')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In a similar fashion we can also compute the Jacobian, in this case as a $6 \\times 4$ matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[0., 0., 0., 0.],\n",
       "       [3., 2., 1., 0.],\n",
       "       [0., 0., 0., 0.],\n",
       "       [0., 0., 0., 0.],\n",
       "       [0., 0., 0., 0.],\n",
       "       [1., 1., 1., 1.]])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.jacob0(robot.qz, endlink='eea')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can generate an ETS for the bottom-branch of the robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Rz(q0) * tx(1) * Rz(q1) * tx(1) * Rz(q4) * tx(1) * Rz(q5) * tz(1)"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p = robot.ets(end='eeb')\n",
    "p"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and we could easily turn this into an unbranched robot represented by an `ERobot` instance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "┌───┬───────┬────────┬───────┬────────────────┐\n",
      "│id │ link  │ parent │ joint │      ETS       │\n",
      "├───┼───────┼────────┼───────┼────────────────┤\n",
      "│ 0\u001b[0m │ link0\u001b[0m │      -\u001b[0m │      \u001b[0m │ Rz(q0)\u001b[0m         │\n",
      "│ 1\u001b[0m │ link1\u001b[0m │  link0\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q1)\u001b[0m │\n",
      "│ 2\u001b[0m │ link2\u001b[0m │  link1\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q4)\u001b[0m │\n",
      "│ 3\u001b[0m │ link3\u001b[0m │  link2\u001b[0m │  None\u001b[0m │ tx(1) * Rz(q5)\u001b[0m │\n",
      "│ 4\u001b[0m │   \u001b[38;5;4m@ee\u001b[0m │  link3\u001b[0m │  None\u001b[0m │ tz(1)\u001b[0m          │\n",
      "└───┴───────┴────────┴───────┴────────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "r = rtb.ERobot(p, checkjindex=False)\n",
    "print(r)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This robot has 5 links but only 4 joint angles which are inherited from the original branched robot: `q0`, `q1`, `q4` and `q5`.  If invoking its `fkine` method you must pass in a vector with at least 6 elements, but only 4 elements will be used.\n",
    "\n",
    "We can also find the transformation between any two links in the robot.  In this case, from one end effector to the other, from link eea to link eeb"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "tz(-1) * Rz(-q3) * tx(-1) * Rz(-q2) * tx(-1) * tx(1) * Rz(q4) * tx(1) * Rz(q5) * tz(1)"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p = robot.ets(start='eea', end='eeb')\n",
    "p"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which is expressed as an ETS.  Note that it is not in minimal form and the terms in the middle `tx(-1) * tx(1)` cancel out.  We can precompile the constants which will take care of this for us"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "C0 * Rz(-q3) * C1 * Rz(-q2) * Rz(q4) * C2 * Rz(q5) * C3"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p.compile()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Graph display variations\n",
    "\n",
    "This robot is entirely revolute, but we can visualize the joint types in the graphical representation by using the `jtype` keyword argument"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\u001b]2;C02D8125PN78:/Users/corkep/code/robotics-toolbox-python/notebooks\u0007\u001b]1;C02D8125PN78\u0007"
     ]
    },
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"158pt\" viewBox=\"0.00 0.00 840.47 157.64\" width=\"840pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\"><g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 153.64)\"><title>G</title><polygon fill=\"white\" points=\"-4,4 -4,-153.64 836.47,-153.64 836.47,4 -4,4\" stroke=\"transparent\"/><!-- BASE --><g class=\"node\" id=\"node1\"><title>BASE</title><polygon fill=\"gray\" points=\"51,-100.65 0,-100.65 0,-49.65 51,-49.65 51,-100.65\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"25.5\" y=\"-71.45\">BASE</text></g><!-- link0 --><g class=\"node\" id=\"node2\"><title>link0</title><ellipse cx=\"153.6\" cy=\"-75.15\" fill=\"none\" rx=\"28.7\" ry=\"28.7\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"153.6\" y=\"-71.45\">link0</text></g><!-- BASE&#45;&gt;link0 --><g class=\"edge\" id=\"edge1\"><title>BASE-&gt;link0</title><path d=\"M61.12,-75.15C78.4,-75.15 99.28,-75.15 116.72,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"51.11,-71.65 61.11,-75.15 51.11,-78.65 51.11,-71.65\" stroke=\"black\"/><ellipse cx=\"120.98\" cy=\"-75.15\" fill=\"black\" rx=\"4\" ry=\"4\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"88\" y=\"-78.95\">Rz(q0)</text></g><!-- link1 --><g class=\"node\" id=\"node3\"><title>link1</title><ellipse cx=\"325.79\" cy=\"-75.15\" fill=\"none\" rx=\"28.7\" ry=\"28.7\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"325.79\" y=\"-71.45\">link1</text></g><!-- link0&#45;&gt;link1 --><g class=\"edge\" id=\"edge2\"><title>link0-&gt;link1</title><path d=\"M192.54,-75.15C221.37,-75.15 260.77,-75.15 289.05,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"182.32,-71.65 192.32,-75.15 182.32,-78.65 182.32,-71.65\" stroke=\"black\"/><ellipse cx=\"293.08\" cy=\"-75.15\" fill=\"black\" rx=\"4\" ry=\"4\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"239.69\" y=\"-78.95\">tx(1) * Rz(q1)</text></g><!-- link2a --><g class=\"node\" id=\"node4\"><title>link2a</title><ellipse cx=\"502.54\" cy=\"-117.15\" fill=\"none\" rx=\"32.49\" ry=\"32.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"502.54\" y=\"-113.45\">link2a</text></g><!-- link1&#45;&gt;link2a --><g class=\"edge\" id=\"edge3\"><title>link1-&gt;link2a</title><path d=\"M363.52,-83.98C392.6,-90.96 433.11,-100.7 462.71,-107.82\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"354.53,-78.21 363.44,-83.95 352.9,-85.02 354.53,-78.21\" stroke=\"black\"/><ellipse cx=\"466.87\" cy=\"-108.81\" fill=\"black\" rx=\"4\" ry=\"4\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"411.89\" y=\"-107.95\">tx(1) * Rz(q2)</text></g><!-- link2b --><g class=\"node\" id=\"node6\"><title>link2b</title><ellipse cx=\"502.54\" cy=\"-33.15\" fill=\"none\" rx=\"33.29\" ry=\"33.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"502.54\" y=\"-29.45\">link2b</text></g><!-- link1&#45;&gt;link2b --><g class=\"edge\" id=\"edge5\"><title>link1-&gt;link2b</title><path d=\"M363.46,-66.33C392.35,-59.39 432.54,-49.73 462.11,-42.62\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"352.9,-65.27 363.44,-66.34 354.53,-72.08 352.9,-65.27\" stroke=\"black\"/><ellipse cx=\"466.26\" cy=\"-41.63\" fill=\"black\" rx=\"4\" ry=\"4\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"411.89\" y=\"-67.95\">tx(1) * Rz(q4)</text></g><!-- link3a --><g class=\"node\" id=\"node5\"><title>link3a</title><ellipse cx=\"683.83\" cy=\"-117.15\" fill=\"none\" rx=\"32.49\" ry=\"32.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"683.83\" y=\"-113.45\">link3a</text></g><!-- link2a&#45;&gt;link3a --><g class=\"edge\" id=\"edge4\"><title>link2a-&gt;link3a</title><path d=\"M545.23,-117.15C574.67,-117.15 613.89,-117.15 642.92,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"535.18,-113.65 545.18,-117.15 535.18,-120.65 535.18,-113.65\" stroke=\"black\"/><ellipse cx=\"647.2\" cy=\"-117.15\" fill=\"black\" rx=\"4\" ry=\"4\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"593.18\" y=\"-120.95\">tx(1) * Rz(q3)</text></g><!-- eea --><g class=\"node\" id=\"node8\"><title>eea</title><ellipse cx=\"805.73\" cy=\"-117.15\" fill=\"none\" rx=\"22.96\" ry=\"22.96\" stroke=\"blue\"/><ellipse cx=\"805.73\" cy=\"-117.15\" fill=\"none\" rx=\"27\" ry=\"27\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"805.73\" y=\"-113.45\">eea</text></g><!-- link3a&#45;&gt;eea --><g class=\"edge\" id=\"edge7\"><title>link3a-&gt;eea</title><path d=\"M716.33,-117.15C732.27,-117.15 751.7,-117.15 768.27,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"768.69,-120.65 778.69,-117.15 768.69,-113.65 768.69,-120.65\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"747.98\" y=\"-120.95\">tz(1)</text></g><!-- link3b --><g class=\"node\" id=\"node7\"><title>link3b</title><ellipse cx=\"683.83\" cy=\"-33.15\" fill=\"none\" rx=\"33.29\" ry=\"33.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"683.83\" y=\"-29.45\">link3b</text></g><!-- link2b&#45;&gt;link3b --><g class=\"edge\" id=\"edge6\"><title>link2b-&gt;link3b</title><path d=\"M546.4,-33.15C575.54,-33.15 613.84,-33.15 642.44,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"536.01,-29.65 546.01,-33.15 536.01,-36.65 536.01,-29.65\" stroke=\"black\"/><ellipse cx=\"646.6\" cy=\"-33.15\" fill=\"black\" rx=\"4\" ry=\"4\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"593.18\" y=\"-36.95\">tx(1) * Rz(q5)</text></g><!-- eeb --><g class=\"node\" id=\"node9\"><title>eeb</title><ellipse cx=\"805.73\" cy=\"-33.15\" fill=\"none\" rx=\"22.96\" ry=\"22.96\" stroke=\"blue\"/><ellipse cx=\"805.73\" cy=\"-33.15\" fill=\"none\" rx=\"27\" ry=\"27\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"805.73\" y=\"-29.45\">eeb</text></g><!-- link3b&#45;&gt;eeb --><g class=\"edge\" id=\"edge8\"><title>link3b-&gt;eeb</title><path d=\"M717.28,-33.15C733.09,-33.15 752.17,-33.15 768.46,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"768.7,-36.65 778.7,-33.15 768.7,-29.65 768.7,-36.65\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"747.98\" y=\"-36.95\">tz(1)</text></g></g></svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.dotfile('planar_y.dot', jtype=True)\n",
    "!dot -Tsvg planar_y.dot > planar_y.svg\n",
    "SVG(filename='planar_y.svg')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and the input to a link frame is a black dot if it is preceded by a revolute joint, and a black box if it  preceded by a prismatic joint.\n",
    "\n",
    "We can also highlight the ETS by placing them in boxes, rather than being just edge labels, using the `etsbox` option"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\u001b]2;C02D8125PN78:/Users/corkep/code/robotics-toolbox-python/notebooks\u0007\u001b]1;C02D8125PN78\u0007"
     ]
    },
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"158pt\" viewBox=\"0.00 0.00 1112.47 157.64\" width=\"1112pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\"><g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 153.64)\"><title>G</title><polygon fill=\"white\" points=\"-4,4 -4,-153.64 1108.47,-153.64 1108.47,4 -4,4\" stroke=\"transparent\"/><!-- BASE --><g class=\"node\" id=\"node1\"><title>BASE</title><polygon fill=\"gray\" points=\"51,-100.65 0,-100.65 0,-49.65 51,-49.65 51,-100.65\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"25.5\" y=\"-71.45\">BASE</text></g><!-- link0_ets --><g class=\"node\" id=\"node3\"><title>link0_ets</title><path d=\"M129,-93.15C129,-93.15 99,-93.15 99,-93.15 93,-93.15 87,-87.15 87,-81.15 87,-81.15 87,-69.15 87,-69.15 87,-63.15 93,-57.15 99,-57.15 99,-57.15 129,-57.15 129,-57.15 135,-57.15 141,-63.15 141,-69.15 141,-69.15 141,-81.15 141,-81.15 141,-87.15 135,-93.15 129,-93.15\" fill=\"none\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"114\" y=\"-71.45\">Rz(q0)</text></g><!-- BASE&#45;&gt;link0_ets --><g class=\"edge\" id=\"edge1\"><title>BASE-&gt;link0_ets</title><path d=\"M51.06,-75.15C59.01,-75.15 68.01,-75.15 76.63,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"76.87,-78.65 86.87,-75.15 76.87,-71.65 76.87,-78.65\" stroke=\"black\"/></g><!-- link0 --><g class=\"node\" id=\"node2\"><title>link0</title><ellipse cx=\"205.6\" cy=\"-75.15\" fill=\"none\" rx=\"28.7\" ry=\"28.7\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"205.6\" y=\"-71.45\">link0</text></g><!-- link1_ets --><g class=\"node\" id=\"node5\"><title>link1_ets</title><path d=\"M353.19,-93.15C353.19,-93.15 282.19,-93.15 282.19,-93.15 276.19,-93.15 270.19,-87.15 270.19,-81.15 270.19,-81.15 270.19,-69.15 270.19,-69.15 270.19,-63.15 276.19,-57.15 282.19,-57.15 282.19,-57.15 353.19,-57.15 353.19,-57.15 359.19,-57.15 365.19,-63.15 365.19,-69.15 365.19,-69.15 365.19,-81.15 365.19,-81.15 365.19,-87.15 359.19,-93.15 353.19,-93.15\" fill=\"none\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"317.69\" y=\"-71.45\">tx(1) * Rz(q1)</text></g><!-- link0&#45;&gt;link1_ets --><g class=\"edge\" id=\"edge3\"><title>link0-&gt;link1_ets</title><path d=\"M234.38,-75.15C242.25,-75.15 251.11,-75.15 260.03,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"260.07,-78.65 270.07,-75.15 260.07,-71.65 260.07,-78.65\" stroke=\"black\"/></g><!-- link0_ets&#45;&gt;link0 --><g class=\"edge\" id=\"edge2\"><title>link0_ets-&gt;link0</title><path d=\"M141.4,-75.15C149.32,-75.15 158.18,-75.15 166.7,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"166.83,-78.65 176.83,-75.15 166.83,-71.65 166.83,-78.65\" stroke=\"black\"/></g><!-- link1 --><g class=\"node\" id=\"node4\"><title>link1</title><ellipse cx=\"429.79\" cy=\"-75.15\" fill=\"none\" rx=\"28.7\" ry=\"28.7\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"429.79\" y=\"-71.45\">link1</text></g><!-- link2a_ets --><g class=\"node\" id=\"node7\"><title>link2a_ets</title><path d=\"M577.39,-127.15C577.39,-127.15 506.39,-127.15 506.39,-127.15 500.39,-127.15 494.39,-121.15 494.39,-115.15 494.39,-115.15 494.39,-103.15 494.39,-103.15 494.39,-97.15 500.39,-91.15 506.39,-91.15 506.39,-91.15 577.39,-91.15 577.39,-91.15 583.39,-91.15 589.39,-97.15 589.39,-103.15 589.39,-103.15 589.39,-115.15 589.39,-115.15 589.39,-121.15 583.39,-127.15 577.39,-127.15\" fill=\"none\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"541.89\" y=\"-105.45\">tx(1) * Rz(q2)</text></g><!-- link1&#45;&gt;link2a_ets --><g class=\"edge\" id=\"edge5\"><title>link1-&gt;link2a_ets</title><path d=\"M457.44,-83.38C465.66,-85.92 475.04,-88.81 484.48,-91.73\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"483.7,-95.15 494.28,-94.75 485.76,-88.46 483.7,-95.15\" stroke=\"black\"/></g><!-- link2b_ets --><g class=\"node\" id=\"node11\"><title>link2b_ets</title><path d=\"M577.39,-66.15C577.39,-66.15 506.39,-66.15 506.39,-66.15 500.39,-66.15 494.39,-60.15 494.39,-54.15 494.39,-54.15 494.39,-42.15 494.39,-42.15 494.39,-36.15 500.39,-30.15 506.39,-30.15 506.39,-30.15 577.39,-30.15 577.39,-30.15 583.39,-30.15 589.39,-36.15 589.39,-42.15 589.39,-42.15 589.39,-54.15 589.39,-54.15 589.39,-60.15 583.39,-66.15 577.39,-66.15\" fill=\"none\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"541.89\" y=\"-44.45\">tx(1) * Rz(q4)</text></g><!-- link1&#45;&gt;link2b_ets --><g class=\"edge\" id=\"edge9\"><title>link1-&gt;link2b_ets</title><path d=\"M457.73,-68.54C465.77,-66.57 474.9,-64.33 484.1,-62.07\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"485.18,-65.41 494.06,-59.63 483.52,-58.61 485.18,-65.41\" stroke=\"black\"/></g><!-- link1_ets&#45;&gt;link1 --><g class=\"edge\" id=\"edge4\"><title>link1_ets-&gt;link1</title><path d=\"M365.39,-75.15C373.96,-75.15 382.81,-75.15 391.07,-75.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"391.17,-78.65 401.17,-75.15 391.17,-71.65 391.17,-78.65\" stroke=\"black\"/></g><!-- link2a --><g class=\"node\" id=\"node6\"><title>link2a</title><ellipse cx=\"658.54\" cy=\"-117.15\" fill=\"none\" rx=\"32.49\" ry=\"32.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"658.54\" y=\"-113.45\">link2a</text></g><!-- link3a_ets --><g class=\"node\" id=\"node9\"><title>link3a_ets</title><path d=\"M810.68,-135.15C810.68,-135.15 739.68,-135.15 739.68,-135.15 733.68,-135.15 727.68,-129.15 727.68,-123.15 727.68,-123.15 727.68,-111.15 727.68,-111.15 727.68,-105.15 733.68,-99.15 739.68,-99.15 739.68,-99.15 810.68,-99.15 810.68,-99.15 816.68,-99.15 822.68,-105.15 822.68,-111.15 822.68,-111.15 822.68,-123.15 822.68,-123.15 822.68,-129.15 816.68,-135.15 810.68,-135.15\" fill=\"none\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"775.18\" y=\"-113.45\">tx(1) * Rz(q3)</text></g><!-- link2a&#45;&gt;link3a_ets --><g class=\"edge\" id=\"edge7\"><title>link2a-&gt;link3a_ets</title><path d=\"M691.17,-117.15C699.35,-117.15 708.42,-117.15 717.47,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"717.63,-120.65 727.63,-117.15 717.63,-113.65 717.63,-120.65\" stroke=\"black\"/></g><!-- link2a_ets&#45;&gt;link2a --><g class=\"edge\" id=\"edge6\"><title>link2a_ets-&gt;link2a</title><path d=\"M589.52,-112.4C598.25,-113.01 607.32,-113.64 615.88,-114.24\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"615.8,-117.74 626.02,-114.95 616.29,-110.76 615.8,-117.74\" stroke=\"black\"/></g><!-- link3a --><g class=\"node\" id=\"node8\"><title>link3a</title><ellipse cx=\"891.83\" cy=\"-117.15\" fill=\"none\" rx=\"32.49\" ry=\"32.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"891.83\" y=\"-113.45\">link3a</text></g><!-- eea_ets --><g class=\"node\" id=\"node15\"><title>eea_ets</title><path d=\"M1002.98,-135.15C1002.98,-135.15 972.98,-135.15 972.98,-135.15 966.98,-135.15 960.98,-129.15 960.98,-123.15 960.98,-123.15 960.98,-111.15 960.98,-111.15 960.98,-105.15 966.98,-99.15 972.98,-99.15 972.98,-99.15 1002.98,-99.15 1002.98,-99.15 1008.98,-99.15 1014.98,-105.15 1014.98,-111.15 1014.98,-111.15 1014.98,-123.15 1014.98,-123.15 1014.98,-129.15 1008.98,-135.15 1002.98,-135.15\" fill=\"none\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"987.98\" y=\"-113.45\">tz(1)</text></g><!-- link3a&#45;&gt;eea_ets --><g class=\"edge\" id=\"edge13\"><title>link3a-&gt;eea_ets</title><path d=\"M924.44,-117.15C932.87,-117.15 942.05,-117.15 950.71,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"950.93,-120.65 960.93,-117.15 950.93,-113.65 950.93,-120.65\" stroke=\"black\"/></g><!-- link3a_ets&#45;&gt;link3a --><g class=\"edge\" id=\"edge8\"><title>link3a_ets-&gt;link3a</title><path d=\"M822.82,-117.15C831.54,-117.15 840.61,-117.15 849.18,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"849.31,-120.65 859.31,-117.15 849.31,-113.65 849.31,-120.65\" stroke=\"black\"/></g><!-- link2b --><g class=\"node\" id=\"node10\"><title>link2b</title><ellipse cx=\"658.54\" cy=\"-33.15\" fill=\"none\" rx=\"33.29\" ry=\"33.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"658.54\" y=\"-29.45\">link2b</text></g><!-- link3b_ets --><g class=\"node\" id=\"node13\"><title>link3b_ets</title><path d=\"M810.68,-51.15C810.68,-51.15 739.68,-51.15 739.68,-51.15 733.68,-51.15 727.68,-45.15 727.68,-39.15 727.68,-39.15 727.68,-27.15 727.68,-27.15 727.68,-21.15 733.68,-15.15 739.68,-15.15 739.68,-15.15 810.68,-15.15 810.68,-15.15 816.68,-15.15 822.68,-21.15 822.68,-27.15 822.68,-27.15 822.68,-39.15 822.68,-39.15 822.68,-45.15 816.68,-51.15 810.68,-51.15\" fill=\"none\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"775.18\" y=\"-29.45\">tx(1) * Rz(q5)</text></g><!-- link2b&#45;&gt;link3b_ets --><g class=\"edge\" id=\"edge11\"><title>link2b-&gt;link3b_ets</title><path d=\"M691.78,-33.15C699.77,-33.15 708.58,-33.15 717.38,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"717.64,-36.65 727.64,-33.15 717.64,-29.65 717.64,-36.65\" stroke=\"black\"/></g><!-- link2b_ets&#45;&gt;link2b --><g class=\"edge\" id=\"edge10\"><title>link2b_ets-&gt;link2b</title><path d=\"M589.52,-42.05C598.07,-40.93 606.95,-39.77 615.36,-38.67\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"615.87,-42.13 625.33,-37.36 614.96,-35.19 615.87,-42.13\" stroke=\"black\"/></g><!-- link3b --><g class=\"node\" id=\"node12\"><title>link3b</title><ellipse cx=\"891.83\" cy=\"-33.15\" fill=\"none\" rx=\"33.29\" ry=\"33.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"891.83\" y=\"-29.45\">link3b</text></g><!-- eeb_ets --><g class=\"node\" id=\"node17\"><title>eeb_ets</title><path d=\"M1002.98,-51.15C1002.98,-51.15 972.98,-51.15 972.98,-51.15 966.98,-51.15 960.98,-45.15 960.98,-39.15 960.98,-39.15 960.98,-27.15 960.98,-27.15 960.98,-21.15 966.98,-15.15 972.98,-15.15 972.98,-15.15 1002.98,-15.15 1002.98,-15.15 1008.98,-15.15 1014.98,-21.15 1014.98,-27.15 1014.98,-27.15 1014.98,-39.15 1014.98,-39.15 1014.98,-45.15 1008.98,-51.15 1002.98,-51.15\" fill=\"none\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"987.98\" y=\"-29.45\">tz(1)</text></g><!-- link3b&#45;&gt;eeb_ets --><g class=\"edge\" id=\"edge15\"><title>link3b-&gt;eeb_ets</title><path d=\"M925.23,-33.15C933.38,-33.15 942.19,-33.15 950.53,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"950.75,-36.65 960.75,-33.15 950.75,-29.65 950.75,-36.65\" stroke=\"black\"/></g><!-- link3b_ets&#45;&gt;link3b --><g class=\"edge\" id=\"edge12\"><title>link3b_ets-&gt;link3b</title><path d=\"M822.82,-33.15C831.27,-33.15 840.05,-33.15 848.38,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"848.63,-36.65 858.63,-33.15 848.63,-29.65 848.63,-36.65\" stroke=\"black\"/></g><!-- eea --><g class=\"node\" id=\"node14\"><title>eea</title><ellipse cx=\"1077.73\" cy=\"-117.15\" fill=\"none\" rx=\"22.96\" ry=\"22.96\" stroke=\"blue\"/><ellipse cx=\"1077.73\" cy=\"-117.15\" fill=\"none\" rx=\"27\" ry=\"27\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1077.73\" y=\"-113.45\">eea</text></g><!-- eea_ets&#45;&gt;eea --><g class=\"edge\" id=\"edge14\"><title>eea_ets-&gt;eea</title><path d=\"M1015.31,-117.15C1023.27,-117.15 1032.16,-117.15 1040.65,-117.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1040.72,-120.65 1050.72,-117.15 1040.72,-113.65 1040.72,-120.65\" stroke=\"black\"/></g><!-- eeb --><g class=\"node\" id=\"node16\"><title>eeb</title><ellipse cx=\"1077.73\" cy=\"-33.15\" fill=\"none\" rx=\"22.96\" ry=\"22.96\" stroke=\"blue\"/><ellipse cx=\"1077.73\" cy=\"-33.15\" fill=\"none\" rx=\"27\" ry=\"27\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1077.73\" y=\"-29.45\">eeb</text></g><!-- eeb_ets&#45;&gt;eeb --><g class=\"edge\" id=\"edge16\"><title>eeb_ets-&gt;eeb</title><path d=\"M1015.31,-33.15C1023.27,-33.15 1032.16,-33.15 1040.65,-33.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1040.72,-36.65 1050.72,-33.15 1040.72,-29.65 1040.72,-36.65\" stroke=\"black\"/></g></g></svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.dotfile('planar_y.dot', etsbox=True)\n",
    "!dot -Tsvg planar_y.dot > planar_y.svg\n",
    "SVG(filename='planar_y.svg')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## PR2 model\n",
    "\n",
    "The PR2 is a very complex branched robot with nearly 90 links"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Caster is an invalid color name, using default color\n",
      "Wheel_l is an invalid color name, using default color\n",
      "Wheel_r is an invalid color name, using default color\n",
      "Caster is an invalid color name, using default color\n",
      "Wheel_l is an invalid color name, using default color\n",
      "Wheel_r is an invalid color name, using default color\n",
      "Caster is an invalid color name, using default color\n",
      "Wheel_l is an invalid color name, using default color\n",
      "Wheel_r is an invalid color name, using default color\n",
      "Caster is an invalid color name, using default color\n",
      "Wheel_l is an invalid color name, using default color\n",
      "Wheel_r is an invalid color name, using default color\n",
      "Grey2 is an invalid color name, using default color\n",
      "RollLinks is an invalid color name, using default color\n",
      "RollLinks is an invalid color name, using default color\n",
      "RollLinks is an invalid color name, using default color\n",
      "RollLinks is an invalid color name, using default color\n",
      "RollLinks is an invalid color name, using default color\n",
      "RollLinks is an invalid color name, using default color\n",
      "\u001b]2;C02D8125PN78:/Users/corkep/code/robotics-toolbox-python/notebooks\u0007\u001b]1;C02D8125PN78\u0007"
     ]
    },
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"4493pt\" viewBox=\"0.00 0.00 6537.08 4492.73\" width=\"6537pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\"><g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 4488.73)\"><title>G</title><polygon fill=\"white\" points=\"-4,4 -4,-4488.73 6533.08,-4488.73 6533.08,4 -4,4\" stroke=\"transparent\"/><!-- BASE --><g class=\"node\" id=\"node1\"><title>BASE</title><polygon fill=\"gray\" points=\"51,-3637.24 0,-3637.24 0,-3586.24 51,-3586.24 51,-3637.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"25.5\" y=\"-3608.04\">BASE</text></g><!-- base_footprint --><g class=\"node\" id=\"node2\"><title>base_footprint</title><ellipse cx=\"149.74\" cy=\"-3611.74\" fill=\"none\" rx=\"61.99\" ry=\"61.99\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.74\" y=\"-3608.04\">base_footprint</text></g><!-- BASE&#45;&gt;base_footprint --><g class=\"edge\" id=\"edge1\"><title>BASE-&gt;base_footprint</title><path d=\"M51.24,-3611.74C59.16,-3611.74 68.36,-3611.74 77.89,-3611.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"77.9,-3615.24 87.9,-3611.74 77.9,-3608.24 77.9,-3615.24\" stroke=\"black\"/></g><!-- base_link --><g class=\"node\" id=\"node3\"><title>base_link</title><ellipse cx=\"341.69\" cy=\"-3611.74\" fill=\"none\" rx=\"44.39\" ry=\"44.39\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"341.69\" y=\"-3608.04\">base_link</text></g><!-- base_footprint&#45;&gt;base_link --><g class=\"edge\" id=\"edge2\"><title>base_footprint-&gt;base_link</title><path d=\"M211.63,-3611.74C235.92,-3611.74 263.64,-3611.74 287.15,-3611.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"287.29,-3615.24 297.29,-3611.74 287.29,-3608.24 287.29,-3615.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"254.49\" y=\"-3615.54\">tz(0.051)</text></g><!-- base_bellow_link --><g class=\"node\" id=\"node4\"><title>base_bellow_link</title><ellipse cx=\"797.67\" cy=\"-4298.74\" fill=\"none\" rx=\"72.6\" ry=\"72.6\" stroke=\"blue\"/><ellipse cx=\"797.67\" cy=\"-4298.74\" fill=\"none\" rx=\"76.59\" ry=\"76.59\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-4295.04\">base_bellow_link</text></g><!-- base_link&#45;&gt;base_bellow_link --><g class=\"edge\" id=\"edge3\"><title>base_link-&gt;base_bellow_link</title><path d=\"M342.8,-3656.19C343.91,-3764.5 351.92,-4038.45 403.88,-4110.74 477,-4212.47 620.04,-4261.27 711.84,-4283.04\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"711.14,-4286.47 721.67,-4285.31 712.72,-4279.65 711.14,-4286.47\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-4273.54\">tx(-0.29) * tz(0.8)</text></g><!-- base_laser_link --><g class=\"node\" id=\"node5\"><title>base_laser_link</title><ellipse cx=\"797.67\" cy=\"-4134.74\" fill=\"none\" rx=\"65.78\" ry=\"65.78\" stroke=\"blue\"/><ellipse cx=\"797.67\" cy=\"-4134.74\" fill=\"none\" rx=\"69.79\" ry=\"69.79\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-4131.04\">base_laser_link</text></g><!-- base_link&#45;&gt;base_laser_link --><g class=\"edge\" id=\"edge4\"><title>base_link-&gt;base_laser_link</title><path d=\"M346.73,-3655.89C354.38,-3731.31 373.03,-3880.88 403.88,-3922.74 482.17,-4028.97 630.11,-4087.68 720.4,-4115.09\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"719.45,-4118.46 730.03,-4117.96 721.45,-4111.75 719.45,-4118.46\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-4095.54\">tx(0.275) * tz(0.252)</text></g><!-- fl_caster_rotation_link --><g class=\"node\" id=\"node6\"><title>fl_caster_rotation_link</title><ellipse cx=\"797.67\" cy=\"-3956.74\" fill=\"none\" rx=\"90.18\" ry=\"90.18\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-3953.04\">fl_caster_rotation_link</text></g><!-- base_link&#45;&gt;fl_caster_rotation_link --><g class=\"edge\" id=\"edge5\"><title>base_link-&gt;fl_caster_rotation_link</title><path d=\"M356.78,-3653.56C366.86,-3678.76 382.47,-3710.22 403.88,-3732.74 488.58,-3821.78 616.57,-3885.69 703.23,-3921.9\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"701.96,-3925.16 712.54,-3925.75 704.64,-3918.69 701.96,-3925.16\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-3907.54\">tx(0.2246) * ty(0.2246) * tz(0.0282) * Rz(q0)</text></g><!-- fr_caster_rotation_link --><g class=\"node\" id=\"node9\"><title>fr_caster_rotation_link</title><ellipse cx=\"797.67\" cy=\"-3757.74\" fill=\"none\" rx=\"90.98\" ry=\"90.98\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-3754.04\">fr_caster_rotation_link</text></g><!-- base_link&#45;&gt;fr_caster_rotation_link --><g class=\"edge\" id=\"edge8\"><title>base_link-&gt;fr_caster_rotation_link</title><path d=\"M384.18,-3625.08C455.57,-3648.04 603.38,-3695.58 701.29,-3727.06\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"700.31,-3730.43 710.9,-3730.16 702.45,-3723.76 700.31,-3730.43\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-3717.54\">tx(0.2246) * ty(-0.2246) * tz(0.0282) * Rz(q3)</text></g><!-- bl_caster_rotation_link --><g class=\"node\" id=\"node12\"><title>bl_caster_rotation_link</title><ellipse cx=\"797.67\" cy=\"-3551.74\" fill=\"none\" rx=\"92.08\" ry=\"92.08\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-3548.04\">bl_caster_rotation_link</text></g><!-- base_link&#45;&gt;bl_caster_rotation_link --><g class=\"edge\" id=\"edge11\"><title>base_link-&gt;bl_caster_rotation_link</title><path d=\"M385.8,-3606.04C456.45,-3596.7 598.71,-3577.9 695.82,-3565.07\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"696.5,-3568.51 705.96,-3563.73 695.59,-3561.57 696.5,-3568.51\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-3606.54\">tx(-0.2246) * ty(0.2246) * tz(0.0282) * Rz(q6)</text></g><!-- br_caster_rotation_link --><g class=\"node\" id=\"node15\"><title>br_caster_rotation_link</title><ellipse cx=\"797.67\" cy=\"-3127.74\" fill=\"none\" rx=\"92.88\" ry=\"92.88\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-3124.04\">br_caster_rotation_link</text></g><!-- base_link&#45;&gt;br_caster_rotation_link --><g class=\"edge\" id=\"edge14\"><title>base_link-&gt;br_caster_rotation_link</title><path d=\"M372.47,-3579.98C443.65,-3504.1 625.2,-3310.55 726.19,-3202.88\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"729.03,-3204.96 733.32,-3195.28 723.93,-3200.18 729.03,-3204.96\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-3544.54\">tx(-0.2246) * ty(-0.2246) * tz(0.0282) * Rz(q9)</text></g><!-- torso_lift_link --><g class=\"node\" id=\"node18\"><title>torso_lift_link</title><ellipse cx=\"797.67\" cy=\"-2268.74\" fill=\"none\" rx=\"61.19\" ry=\"61.19\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-2265.04\">torso_lift_link</text></g><!-- base_link&#45;&gt;torso_lift_link --><g class=\"edge\" id=\"edge17\"><title>base_link-&gt;torso_lift_link</title><path d=\"M347.11,-3567.86C354.69,-3499.39 372.46,-3363.42 403.88,-3251.74 504.68,-2893.48 691.8,-2486.9 765.59,-2332.63\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"768.91,-2333.81 770.08,-2323.28 762.6,-2330.78 768.91,-2333.81\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-3255.54\">tx(-0.05) * tz(0.739675) * tz(q12)</text></g><!-- torso_lift_motor_screw_link --><g class=\"node\" id=\"node89\"><title>torso_lift_motor_screw_link</title><ellipse cx=\"797.67\" cy=\"-2073.74\" fill=\"none\" rx=\"111.59\" ry=\"111.59\" stroke=\"blue\"/><ellipse cx=\"797.67\" cy=\"-2073.74\" fill=\"none\" rx=\"115.58\" ry=\"115.58\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"797.67\" y=\"-2070.04\">torso_lift_motor_screw_link</text></g><!-- base_link&#45;&gt;torso_lift_motor_screw_link --><g class=\"edge\" id=\"edge88\"><title>base_link-&gt;torso_lift_motor_screw_link</title><path d=\"M343.66,-3567.47C348.02,-3378.53 367.2,-2644.92 403.88,-2548.74 465.42,-2387.39 605.46,-2240.7 700.58,-2154.32\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"703.13,-2156.73 708.21,-2147.43 698.44,-2151.53 703.13,-2156.73\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"533.88\" y=\"-2552.54\">tx(-0.15) * tz(0.7) * Rz(q44)</text></g><!-- fl_caster_l_wheel_link --><g class=\"node\" id=\"node7\"><title>fl_caster_l_wheel_link</title><ellipse cx=\"1335\" cy=\"-4389.74\" fill=\"none\" rx=\"90.98\" ry=\"90.98\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-4389.74\" fill=\"none\" rx=\"94.98\" ry=\"94.98\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-4386.04\">fl_caster_l_wheel_link</text></g><!-- fl_caster_rotation_link&#45;&gt;fl_caster_l_wheel_link --><g class=\"edge\" id=\"edge6\"><title>fl_caster_rotation_link-&gt;fl_caster_l_wheel_link</title><path d=\"M872.51,-4007.89C888.06,-4021.79 902.93,-4037.97 913.46,-4055.74 932.93,-4088.58 909.49,-4108.51 931.46,-4139.74 1006.4,-4246.26 1142.6,-4316.64 1235.63,-4354.85\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1234.47,-4358.15 1245.05,-4358.67 1237.1,-4351.67 1234.47,-4358.15\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-4342.54\">ty(0.049) * Ry(q1)</text></g><!-- fl_caster_r_wheel_link --><g class=\"node\" id=\"node8\"><title>fl_caster_r_wheel_link</title><ellipse cx=\"1335\" cy=\"-4181.74\" fill=\"none\" rx=\"90.98\" ry=\"90.98\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-4181.74\" fill=\"none\" rx=\"94.98\" ry=\"94.98\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-4178.04\">fl_caster_r_wheel_link</text></g><!-- fl_caster_rotation_link&#45;&gt;fl_caster_r_wheel_link --><g class=\"edge\" id=\"edge7\"><title>fl_caster_rotation_link-&gt;fl_caster_r_wheel_link</title><path d=\"M881.14,-3991.4C977.27,-4031.81 1135.91,-4098.48 1237.63,-4141.24\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1236.42,-4144.52 1246.99,-4145.17 1239.13,-4138.07 1236.42,-4144.52\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-4124.54\">ty(-0.049) * Ry(q2)</text></g><!-- fr_caster_l_wheel_link --><g class=\"node\" id=\"node10\"><title>fr_caster_l_wheel_link</title><ellipse cx=\"1335\" cy=\"-3972.74\" fill=\"none\" rx=\"91.78\" ry=\"91.78\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-3972.74\" fill=\"none\" rx=\"95.78\" ry=\"95.78\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-3969.04\">fr_caster_l_wheel_link</text></g><!-- fr_caster_rotation_link&#45;&gt;fr_caster_l_wheel_link --><g class=\"edge\" id=\"edge9\"><title>fr_caster_rotation_link-&gt;fr_caster_l_wheel_link</title><path d=\"M882.32,-3791.33C978.27,-3829.87 1135.39,-3892.97 1236.73,-3933.67\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1235.47,-3936.94 1246.05,-3937.42 1238.08,-3930.44 1235.47,-3936.94\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-3918.54\">ty(0.049) * Ry(q4)</text></g><!-- fr_caster_r_wheel_link --><g class=\"node\" id=\"node11\"><title>fr_caster_r_wheel_link</title><ellipse cx=\"1335\" cy=\"-3762.74\" fill=\"none\" rx=\"92.09\" ry=\"92.09\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-3762.74\" fill=\"none\" rx=\"96.08\" ry=\"96.08\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-3759.04\">fr_caster_r_wheel_link</text></g><!-- fr_caster_rotation_link&#45;&gt;fr_caster_r_wheel_link --><g class=\"edge\" id=\"edge10\"><title>fr_caster_rotation_link-&gt;fr_caster_r_wheel_link</title><path d=\"M888.88,-3758.58C982.65,-3759.46 1129.31,-3760.83 1228.42,-3761.75\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1228.42,-3765.25 1238.45,-3761.85 1228.49,-3758.25 1228.42,-3765.25\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-3764.54\">ty(-0.049) * Ry(q5)</text></g><!-- bl_caster_l_wheel_link --><g class=\"node\" id=\"node13\"><title>bl_caster_l_wheel_link</title><ellipse cx=\"1335\" cy=\"-3551.74\" fill=\"none\" rx=\"92.89\" ry=\"92.89\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-3551.74\" fill=\"none\" rx=\"96.88\" ry=\"96.88\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-3548.04\">bl_caster_l_wheel_link</text></g><!-- bl_caster_rotation_link&#45;&gt;bl_caster_l_wheel_link --><g class=\"edge\" id=\"edge12\"><title>bl_caster_rotation_link-&gt;bl_caster_l_wheel_link</title><path d=\"M890.09,-3551.74C983.68,-3551.74 1129.03,-3551.74 1227.7,-3551.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1227.99,-3555.24 1237.99,-3551.74 1227.99,-3548.24 1227.99,-3555.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-3555.54\">ty(0.049) * Ry(q7)</text></g><!-- bl_caster_r_wheel_link --><g class=\"node\" id=\"node14\"><title>bl_caster_r_wheel_link</title><ellipse cx=\"1335\" cy=\"-3339.74\" fill=\"none\" rx=\"92.89\" ry=\"92.89\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-3339.74\" fill=\"none\" rx=\"96.88\" ry=\"96.88\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-3336.04\">bl_caster_r_wheel_link</text></g><!-- bl_caster_rotation_link&#45;&gt;bl_caster_r_wheel_link --><g class=\"edge\" id=\"edge13\"><title>bl_caster_rotation_link-&gt;bl_caster_r_wheel_link</title><path d=\"M883.79,-3518.03C979.25,-3480.23 1133.85,-3419 1234.73,-3379.05\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1236.31,-3382.19 1244.32,-3375.26 1233.73,-3375.68 1236.31,-3382.19\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-3498.54\">ty(-0.049) * Ry(q8)</text></g><!-- br_caster_l_wheel_link --><g class=\"node\" id=\"node16\"><title>br_caster_l_wheel_link</title><ellipse cx=\"1335\" cy=\"-3127.74\" fill=\"none\" rx=\"92.89\" ry=\"92.89\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-3127.74\" fill=\"none\" rx=\"96.88\" ry=\"96.88\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-3124.04\">br_caster_l_wheel_link</text></g><!-- br_caster_rotation_link&#45;&gt;br_caster_l_wheel_link --><g class=\"edge\" id=\"edge15\"><title>br_caster_rotation_link-&gt;br_caster_l_wheel_link</title><path d=\"M890.7,-3127.74C984.24,-3127.74 1129.04,-3127.74 1227.49,-3127.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1227.76,-3131.24 1237.76,-3127.74 1227.76,-3124.24 1227.76,-3131.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-3131.54\">ty(0.049) * Ry(q10)</text></g><!-- br_caster_r_wheel_link --><g class=\"node\" id=\"node17\"><title>br_caster_r_wheel_link</title><ellipse cx=\"1335\" cy=\"-2914.74\" fill=\"none\" rx=\"93.68\" ry=\"93.68\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-2914.74\" fill=\"none\" rx=\"97.68\" ry=\"97.68\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-2911.04\">br_caster_r_wheel_link</text></g><!-- br_caster_rotation_link&#45;&gt;br_caster_r_wheel_link --><g class=\"edge\" id=\"edge16\"><title>br_caster_rotation_link-&gt;br_caster_r_wheel_link</title><path d=\"M884.38,-3093.64C979.81,-3055.67 1133.84,-2994.38 1234.51,-2954.33\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1236.08,-2957.47 1244.08,-2950.52 1233.49,-2950.96 1236.08,-2957.47\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-3073.54\">ty(-0.049) * Ry(q11)</text></g><!-- l_torso_lift_side_plate_link --><g class=\"node\" id=\"node19\"><title>l_torso_lift_side_plate_link</title><ellipse cx=\"1335\" cy=\"-2687.74\" fill=\"none\" rx=\"107.78\" ry=\"107.78\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-2687.74\" fill=\"none\" rx=\"111.78\" ry=\"111.78\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-2684.04\">l_torso_lift_side_plate_link</text></g><!-- torso_lift_link&#45;&gt;l_torso_lift_side_plate_link --><g class=\"edge\" id=\"edge18\"><title>torso_lift_link-&gt;l_torso_lift_side_plate_link</title><path d=\"M836.34,-2316.51C861.56,-2346.96 896.43,-2386.26 931.46,-2416.74 1025.32,-2498.4 1144.85,-2576.03 1229.19,-2627.1\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1227.39,-2630.1 1237.76,-2632.27 1231,-2624.11 1227.39,-2630.1\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-2614.54\">tx(0.0535) * ty(0.209285) * tz(0.176625)</text></g><!-- r_torso_lift_side_plate_link --><g class=\"node\" id=\"node20\"><title>r_torso_lift_side_plate_link</title><ellipse cx=\"1335\" cy=\"-2445.74\" fill=\"none\" rx=\"108.58\" ry=\"108.58\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-2445.74\" fill=\"none\" rx=\"112.58\" ry=\"112.58\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-2442.04\">r_torso_lift_side_plate_link</text></g><!-- torso_lift_link&#45;&gt;r_torso_lift_side_plate_link --><g class=\"edge\" id=\"edge19\"><title>torso_lift_link-&gt;r_torso_lift_side_plate_link</title><path d=\"M855.96,-2287.68C942.06,-2316.15 1106.6,-2370.55 1218.32,-2407.49\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1217.35,-2410.86 1227.95,-2410.67 1219.55,-2404.21 1217.35,-2410.86\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-2401.54\">tx(0.0535) * ty(-0.209285) * tz(0.176625)</text></g><!-- imu_link --><g class=\"node\" id=\"node21\"><title>imu_link</title><ellipse cx=\"1335\" cy=\"-2268.74\" fill=\"none\" rx=\"42.8\" ry=\"42.8\" stroke=\"blue\"/><ellipse cx=\"1335\" cy=\"-2268.74\" fill=\"none\" rx=\"46.79\" ry=\"46.79\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-2265.04\">imu_link</text></g><!-- torso_lift_link&#45;&gt;imu_link --><g class=\"edge\" id=\"edge20\"><title>torso_lift_link-&gt;imu_link</title><path d=\"M858.78,-2268.74C963.09,-2268.74 1176.17,-2268.74 1277.74,-2268.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1277.8,-2272.24 1287.8,-2268.74 1277.8,-2265.24 1277.8,-2272.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-2272.54\">tx(-0.02977) * ty(-0.1497) * tz(0.164) * Ry(180°)</text></g><!-- head_pan_link --><g class=\"node\" id=\"node22\"><title>head_pan_link</title><ellipse cx=\"1335\" cy=\"-2141.74\" fill=\"none\" rx=\"62.29\" ry=\"62.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-2138.04\">head_pan_link</text></g><!-- torso_lift_link&#45;&gt;head_pan_link --><g class=\"edge\" id=\"edge21\"><title>torso_lift_link-&gt;head_pan_link</title><path d=\"M857.24,-2254.85C956.58,-2231.28 1157.7,-2183.57 1264.19,-2158.3\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1265.28,-2161.64 1274.2,-2155.93 1263.66,-2154.83 1265.28,-2161.64\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-2237.54\">tx(-0.01707) * tz(0.38145) * Rz(q13)</text></g><!-- laser_tilt_mount_link --><g class=\"node\" id=\"node43\"><title>laser_tilt_mount_link</title><ellipse cx=\"1335\" cy=\"-1974.74\" fill=\"none\" rx=\"87.18\" ry=\"87.18\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-1971.04\">laser_tilt_mount_link</text></g><!-- torso_lift_link&#45;&gt;laser_tilt_mount_link --><g class=\"edge\" id=\"edge42\"><title>torso_lift_link-&gt;laser_tilt_mount_link</title><path d=\"M853.73,-2244.23C874.58,-2232.8 897.18,-2217.53 913.46,-2198.74 926.76,-2183.39 917.24,-2171.24 931.46,-2156.74 1016.47,-2070.04 1150.28,-2020.97 1240.03,-1996.14\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1241.2,-1999.45 1249.93,-1993.45 1239.36,-1992.7 1241.2,-1999.45\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-2160.54\">tx(0.09893) * tz(0.227) * Ry(q15)</text></g><!-- r_shoulder_pan_link --><g class=\"node\" id=\"node45\"><title>r_shoulder_pan_link</title><ellipse cx=\"1335\" cy=\"-1786.74\" fill=\"none\" rx=\"83.39\" ry=\"83.39\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-1783.04\">r_shoulder_pan_link</text></g><!-- torso_lift_link&#45;&gt;r_shoulder_pan_link --><g class=\"edge\" id=\"edge44\"><title>torso_lift_link-&gt;r_shoulder_pan_link</title><path d=\"M855.2,-2247.56C877.06,-2236.42 900.01,-2220.47 913.46,-2198.74 938.12,-2158.92 905.19,-2027.51 931.46,-1988.74 1003.38,-1882.62 1148.28,-1829.58 1243.19,-1805.15\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1244.23,-1808.5 1253.07,-1802.66 1242.52,-1801.71 1244.23,-1808.5\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-1992.54\">ty(-0.188) * Rz(q16)</text></g><!-- l_shoulder_pan_link --><g class=\"node\" id=\"node67\"><title>l_shoulder_pan_link</title><ellipse cx=\"1335\" cy=\"-1350.74\" fill=\"none\" rx=\"83.39\" ry=\"83.39\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1335\" y=\"-1347.04\">l_shoulder_pan_link</text></g><!-- torso_lift_link&#45;&gt;l_shoulder_pan_link --><g class=\"edge\" id=\"edge66\"><title>torso_lift_link-&gt;l_shoulder_pan_link</title><path d=\"M855.34,-2247.97C877.36,-2236.87 900.39,-2220.84 913.46,-2198.74 958.54,-2122.54 898.01,-1882.71 931.46,-1800.74 1000.81,-1630.83 1163.58,-1482.85 1259.55,-1406.27\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1261.82,-1408.94 1267.48,-1399.98 1257.47,-1403.45 1261.82,-1408.94\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1067.96\" y=\"-1804.54\">ty(0.188) * Rz(q30)</text></g><!-- head_tilt_link --><g class=\"node\" id=\"node23\"><title>head_tilt_link</title><ellipse cx=\"1674.78\" cy=\"-2318.74\" fill=\"none\" rx=\"59.59\" ry=\"59.59\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1674.78\" y=\"-2315.04\">head_tilt_link</text></g><!-- head_pan_link&#45;&gt;head_tilt_link --><g class=\"edge\" id=\"edge22\"><title>head_pan_link-&gt;head_tilt_link</title><path d=\"M1390.65,-2170.37C1451.33,-2202.17 1549.07,-2253.39 1612.36,-2286.55\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1610.96,-2289.77 1621.44,-2291.31 1614.21,-2283.57 1610.96,-2289.77\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1520.54\" y=\"-2269.54\">tx(0.068) * Ry(q14)</text></g><!-- head_plate_frame --><g class=\"node\" id=\"node24\"><title>head_plate_frame</title><ellipse cx=\"2011.02\" cy=\"-2459.74\" fill=\"none\" rx=\"74.19\" ry=\"74.19\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2011.02\" y=\"-2456.04\">head_plate_frame</text></g><!-- head_tilt_link&#45;&gt;head_plate_frame --><g class=\"edge\" id=\"edge23\"><title>head_tilt_link-&gt;head_plate_frame</title><path d=\"M1730.23,-2341.71C1785.3,-2364.94 1870.93,-2401.06 1932.96,-2427.23\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1931.74,-2430.52 1942.31,-2431.18 1934.46,-2424.07 1931.74,-2430.52\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1838.03\" y=\"-2417.54\">tx(0.0232) * tz(0.0645)</text></g><!-- sensor_mount_link --><g class=\"node\" id=\"node25\"><title>sensor_mount_link</title><ellipse cx=\"2366.75\" cy=\"-2752.74\" fill=\"none\" rx=\"78.79\" ry=\"78.79\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2366.75\" y=\"-2749.04\">sensor_mount_link</text></g><!-- head_plate_frame&#45;&gt;sensor_mount_link --><g class=\"edge\" id=\"edge24\"><title>head_plate_frame-&gt;sensor_mount_link</title><path d=\"M2068.85,-2506.82C2131.16,-2558.42 2231.09,-2641.2 2297.57,-2696.27\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2295.57,-2699.16 2305.51,-2702.84 2300.04,-2693.76 2295.57,-2699.16\" stroke=\"black\"/></g><!-- projector_wg6802418_frame --><g class=\"node\" id=\"node41\"><title>projector_wg6802418_frame</title><ellipse cx=\"2366.75\" cy=\"-2459.74\" fill=\"none\" rx=\"113.98\" ry=\"113.98\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2366.75\" y=\"-2456.04\">projector_wg6802418_frame</text></g><!-- head_plate_frame&#45;&gt;projector_wg6802418_frame --><g class=\"edge\" id=\"edge40\"><title>head_plate_frame-&gt;projector_wg6802418_frame</title><path d=\"M2085.31,-2459.74C2130.75,-2459.74 2190.27,-2459.74 2242.78,-2459.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2242.84,-2463.24 2252.84,-2459.74 2242.84,-2456.24 2242.84,-2463.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2177.51\" y=\"-2463.54\">ty(0.11) * tz(0.0546)</text></g><!-- high_def_frame --><g class=\"node\" id=\"node26\"><title>high_def_frame</title><ellipse cx=\"2860.33\" cy=\"-2953.74\" fill=\"none\" rx=\"67.69\" ry=\"67.69\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2860.33\" y=\"-2950.04\">high_def_frame</text></g><!-- sensor_mount_link&#45;&gt;high_def_frame --><g class=\"edge\" id=\"edge25\"><title>sensor_mount_link-&gt;high_def_frame</title><path d=\"M2439.76,-2782.18C2533.09,-2820.34 2694.87,-2886.5 2787.91,-2924.54\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2786.67,-2927.81 2797.25,-2928.36 2789.32,-2921.33 2786.67,-2927.81\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2600.49\" y=\"-2891.54\">tx(0.046457) * ty(-0.11) * tz(0.0546)</text></g><!-- double_stereo_link --><g class=\"node\" id=\"node28\"><title>double_stereo_link</title><ellipse cx=\"2860.33\" cy=\"-2752.74\" fill=\"none\" rx=\"77.99\" ry=\"77.99\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2860.33\" y=\"-2749.04\">double_stereo_link</text></g><!-- sensor_mount_link&#45;&gt;double_stereo_link --><g class=\"edge\" id=\"edge27\"><title>sensor_mount_link-&gt;double_stereo_link</title><path d=\"M2445.64,-2752.74C2534.49,-2752.74 2679.42,-2752.74 2772.02,-2752.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2772.21,-2756.24 2782.21,-2752.74 2772.21,-2749.24 2772.21,-2756.24\" stroke=\"black\"/></g><!-- high_def_optical_frame --><g class=\"node\" id=\"node27\"><title>high_def_optical_frame</title><ellipse cx=\"3314.71\" cy=\"-2965.74\" fill=\"none\" rx=\"95.58\" ry=\"95.58\" stroke=\"blue\"/><ellipse cx=\"3314.71\" cy=\"-2965.74\" fill=\"none\" rx=\"99.58\" ry=\"99.58\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3314.71\" y=\"-2962.04\">high_def_optical_frame</text></g><!-- high_def_frame&#45;&gt;high_def_optical_frame --><g class=\"edge\" id=\"edge26\"><title>high_def_frame-&gt;high_def_optical_frame</title><path d=\"M2928.08,-2955.51C3000.9,-2957.44 3118.56,-2960.56 3204.8,-2962.85\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3204.8,-2966.35 3214.89,-2963.12 3204.98,-2959.35 3204.8,-2966.35\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3107.67\" y=\"-2965.54\">Rz(-90°) * Rx(-90°)</text></g><!-- wide_stereo_link --><g class=\"node\" id=\"node29\"><title>wide_stereo_link</title><ellipse cx=\"3314.71\" cy=\"-2776.74\" fill=\"none\" rx=\"71.49\" ry=\"71.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3314.71\" y=\"-2773.04\">wide_stereo_link</text></g><!-- double_stereo_link&#45;&gt;wide_stereo_link --><g class=\"edge\" id=\"edge28\"><title>double_stereo_link-&gt;wide_stereo_link</title><path d=\"M2938.33,-2756.82C3020.28,-2761.17 3149.41,-2768.02 3232.83,-2772.45\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3232.85,-2775.95 3243.02,-2772.99 3233.22,-2768.96 3232.85,-2775.95\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3107.67\" y=\"-2773.54\">tx(0.045) * ty(0.03) * tz(0.0501)</text></g><!-- narrow_stereo_link --><g class=\"node\" id=\"node35\"><title>narrow_stereo_link</title><ellipse cx=\"3314.71\" cy=\"-2553.74\" fill=\"none\" rx=\"79.09\" ry=\"79.09\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3314.71\" y=\"-2550.04\">narrow_stereo_link</text></g><!-- double_stereo_link&#45;&gt;narrow_stereo_link --><g class=\"edge\" id=\"edge34\"><title>double_stereo_link-&gt;narrow_stereo_link</title><path d=\"M2932.01,-2721.65C3013.41,-2685.84 3146.89,-2627.12 3232.49,-2589.47\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3233.91,-2592.67 3241.65,-2585.44 3231.09,-2586.26 3233.91,-2592.67\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3107.67\" y=\"-2685.54\">tx(0.045) * ty(0.06) * tz(0.0501)</text></g><!-- wide_stereo_optical_frame --><g class=\"node\" id=\"node30\"><title>wide_stereo_optical_frame</title><ellipse cx=\"3855.09\" cy=\"-3100.74\" fill=\"none\" rx=\"106.68\" ry=\"106.68\" stroke=\"blue\"/><ellipse cx=\"3855.09\" cy=\"-3100.74\" fill=\"none\" rx=\"110.68\" ry=\"110.68\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-3097.04\">wide_stereo_optical_frame</text></g><!-- wide_stereo_link&#45;&gt;wide_stereo_optical_frame --><g class=\"edge\" id=\"edge29\"><title>wide_stereo_link-&gt;wide_stereo_optical_frame</title><path d=\"M3376.42,-2813.28C3467.32,-2867.98 3639.94,-2971.87 3750.88,-3038.63\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3749.38,-3041.81 3759.75,-3043.97 3752.99,-3035.81 3749.38,-3041.81\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3560.25\" y=\"-2999.54\">Rz(-90°) * Rx(-90°)</text></g><!-- wide_stereo_l_stereo_camera_frame --><g class=\"node\" id=\"node31\"><title>wide_stereo_l_stereo_camera_frame</title><ellipse cx=\"3855.09\" cy=\"-2830.74\" fill=\"none\" rx=\"141.07\" ry=\"141.07\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-2827.04\">wide_stereo_l_stereo_camera_frame</text></g><!-- wide_stereo_link&#45;&gt;wide_stereo_l_stereo_camera_frame --><g class=\"edge\" id=\"edge30\"><title>wide_stereo_link-&gt;wide_stereo_l_stereo_camera_frame</title><path d=\"M3386.09,-2783.8C3466.58,-2791.87 3600.65,-2805.32 3704.71,-2815.76\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3704.38,-2819.24 3714.68,-2816.76 3705.08,-2812.28 3704.38,-2819.24\" stroke=\"black\"/></g><!-- wide_stereo_l_stereo_camera_optical_frame --><g class=\"node\" id=\"node32\"><title>wide_stereo_l_stereo_camera_optical_frame</title><ellipse cx=\"4330.71\" cy=\"-3163.74\" fill=\"none\" rx=\"168.97\" ry=\"168.97\" stroke=\"blue\"/><ellipse cx=\"4330.71\" cy=\"-3163.74\" fill=\"none\" rx=\"172.97\" ry=\"172.97\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-3160.04\">wide_stereo_l_stereo_camera_optical_frame</text></g><!-- wide_stereo_l_stereo_camera_frame&#45;&gt;wide_stereo_l_stereo_camera_optical_frame --><g class=\"edge\" id=\"edge31\"><title>wide_stereo_l_stereo_camera_frame-&gt;wide_stereo_l_stereo_camera_optical_frame</title><path d=\"M3971.04,-2911.56C4034.05,-2955.87 4112.88,-3011.29 4180.58,-3058.89\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4178.59,-3061.77 4188.78,-3064.66 4182.62,-3056.04 4178.59,-3061.77\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-3020.54\">Rz(-90°) * Rx(-90°)</text></g><!-- wide_stereo_r_stereo_camera_frame --><g class=\"node\" id=\"node33\"><title>wide_stereo_r_stereo_camera_frame</title><ellipse cx=\"4330.71\" cy=\"-2830.74\" fill=\"none\" rx=\"141.88\" ry=\"141.88\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-2827.04\">wide_stereo_r_stereo_camera_frame</text></g><!-- wide_stereo_l_stereo_camera_frame&#45;&gt;wide_stereo_r_stereo_camera_frame --><g class=\"edge\" id=\"edge32\"><title>wide_stereo_l_stereo_camera_frame-&gt;wide_stereo_r_stereo_camera_frame</title><path d=\"M3996.27,-2830.74C4053.64,-2830.74 4120.15,-2830.74 4178.6,-2830.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4178.66,-2834.24 4188.66,-2830.74 4178.66,-2827.24 4178.66,-2834.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-2834.54\">ty(-0.09)</text></g><!-- wide_stereo_r_stereo_camera_optical_frame --><g class=\"node\" id=\"node34\"><title>wide_stereo_r_stereo_camera_optical_frame</title><ellipse cx=\"4838.93\" cy=\"-2830.74\" fill=\"none\" rx=\"169.77\" ry=\"169.77\" stroke=\"blue\"/><ellipse cx=\"4838.93\" cy=\"-2830.74\" fill=\"none\" rx=\"173.77\" ry=\"173.77\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4838.93\" y=\"-2827.04\">wide_stereo_r_stereo_camera_optical_frame</text></g><!-- wide_stereo_r_stereo_camera_frame&#45;&gt;wide_stereo_r_stereo_camera_optical_frame --><g class=\"edge\" id=\"edge33\"><title>wide_stereo_r_stereo_camera_frame-&gt;wide_stereo_r_stereo_camera_optical_frame</title><path d=\"M4472.72,-2830.74C4529.13,-2830.74 4594.86,-2830.74 4654.88,-2830.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4655.24,-2834.24 4665.24,-2830.74 4655.24,-2827.24 4655.24,-2834.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4584.49\" y=\"-2834.54\">Rz(-90°) * Rx(-90°)</text></g><!-- narrow_stereo_optical_frame --><g class=\"node\" id=\"node36\"><title>narrow_stereo_optical_frame</title><ellipse cx=\"3855.09\" cy=\"-2553.74\" fill=\"none\" rx=\"114.28\" ry=\"114.28\" stroke=\"blue\"/><ellipse cx=\"3855.09\" cy=\"-2553.74\" fill=\"none\" rx=\"118.28\" ry=\"118.28\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-2550.04\">narrow_stereo_optical_frame</text></g><!-- narrow_stereo_link&#45;&gt;narrow_stereo_optical_frame --><g class=\"edge\" id=\"edge35\"><title>narrow_stereo_link-&gt;narrow_stereo_optical_frame</title><path d=\"M3394.28,-2553.74C3481.42,-2553.74 3623.28,-2553.74 3726.41,-2553.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3726.59,-2557.24 3736.59,-2553.74 3726.59,-2550.24 3726.59,-2557.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3560.25\" y=\"-2557.54\">Rz(-90°) * Rx(-90°)</text></g><!-- narrow_stereo_l_stereo_camera_frame --><g class=\"node\" id=\"node37\"><title>narrow_stereo_l_stereo_camera_frame</title><ellipse cx=\"3855.09\" cy=\"-2268.74\" fill=\"none\" rx=\"148.67\" ry=\"148.67\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-2265.04\">narrow_stereo_l_stereo_camera_frame</text></g><!-- narrow_stereo_link&#45;&gt;narrow_stereo_l_stereo_camera_frame --><g class=\"edge\" id=\"edge36\"><title>narrow_stereo_link-&gt;narrow_stereo_l_stereo_camera_frame</title><path d=\"M3385.27,-2516.92C3468.06,-2473.09 3608.37,-2398.81 3713.97,-2342.91\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3715.91,-2345.85 3723.11,-2338.07 3712.63,-2339.66 3715.91,-2345.85\" stroke=\"black\"/></g><!-- narrow_stereo_l_stereo_camera_optical_frame --><g class=\"node\" id=\"node38\"><title>narrow_stereo_l_stereo_camera_optical_frame</title><ellipse cx=\"4330.71\" cy=\"-2490.74\" fill=\"none\" rx=\"176.57\" ry=\"176.57\" stroke=\"blue\"/><ellipse cx=\"4330.71\" cy=\"-2490.74\" fill=\"none\" rx=\"180.57\" ry=\"180.57\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-2487.04\">narrow_stereo_l_stereo_camera_optical_frame</text></g><!-- narrow_stereo_l_stereo_camera_frame&#45;&gt;narrow_stereo_l_stereo_camera_optical_frame --><g class=\"edge\" id=\"edge37\"><title>narrow_stereo_l_stereo_camera_frame-&gt;narrow_stereo_l_stereo_camera_optical_frame</title><path d=\"M3990.22,-2331.61C4042.17,-2355.96 4102.29,-2384.14 4157.38,-2409.96\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4155.99,-2413.18 4166.53,-2414.25 4158.96,-2406.84 4155.99,-2413.18\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-2396.54\">Rz(-90°) * Rx(-90°)</text></g><!-- narrow_stereo_r_stereo_camera_frame --><g class=\"node\" id=\"node39\"><title>narrow_stereo_r_stereo_camera_frame</title><ellipse cx=\"4330.71\" cy=\"-2142.74\" fill=\"none\" rx=\"149.47\" ry=\"149.47\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-2139.04\">narrow_stereo_r_stereo_camera_frame</text></g><!-- narrow_stereo_l_stereo_camera_frame&#45;&gt;narrow_stereo_r_stereo_camera_frame --><g class=\"edge\" id=\"edge38\"><title>narrow_stereo_l_stereo_camera_frame-&gt;narrow_stereo_r_stereo_camera_frame</title><path d=\"M3999.15,-2230.68C4055.01,-2215.82 4119.17,-2198.75 4176.02,-2183.63\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4177.04,-2186.98 4185.81,-2181.02 4175.24,-2180.21 4177.04,-2186.98\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-2227.54\">ty(-0.09)</text></g><!-- narrow_stereo_r_stereo_camera_optical_frame --><g class=\"node\" id=\"node40\"><title>narrow_stereo_r_stereo_camera_optical_frame</title><ellipse cx=\"4838.93\" cy=\"-2158.74\" fill=\"none\" rx=\"177.37\" ry=\"177.37\" stroke=\"blue\"/><ellipse cx=\"4838.93\" cy=\"-2158.74\" fill=\"none\" rx=\"181.37\" ry=\"181.37\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4838.93\" y=\"-2155.04\">narrow_stereo_r_stereo_camera_optical_frame</text></g><!-- narrow_stereo_r_stereo_camera_frame&#45;&gt;narrow_stereo_r_stereo_camera_optical_frame --><g class=\"edge\" id=\"edge39\"><title>narrow_stereo_r_stereo_camera_frame-&gt;narrow_stereo_r_stereo_camera_optical_frame</title><path d=\"M4480.17,-2147.43C4532.35,-2149.08 4591.74,-2150.96 4646.92,-2152.7\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4647.07,-2156.21 4657.18,-2153.03 4647.29,-2149.21 4647.07,-2156.21\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4584.49\" y=\"-2155.54\">Rz(-90°) * Rx(-90°)</text></g><!-- projector_wg6802418_child_frame --><g class=\"node\" id=\"node42\"><title>projector_wg6802418_child_frame</title><ellipse cx=\"2860.33\" cy=\"-2459.74\" fill=\"none\" rx=\"135.68\" ry=\"135.68\" stroke=\"blue\"/><ellipse cx=\"2860.33\" cy=\"-2459.74\" fill=\"none\" rx=\"139.68\" ry=\"139.68\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2860.33\" y=\"-2456.04\">projector_wg6802418_child_frame</text></g><!-- projector_wg6802418_frame&#45;&gt;projector_wg6802418_child_frame --><g class=\"edge\" id=\"edge41\"><title>projector_wg6802418_frame-&gt;projector_wg6802418_child_frame</title><path d=\"M2480.53,-2459.74C2548.62,-2459.74 2636.19,-2459.74 2709.98,-2459.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2710.3,-2463.24 2720.3,-2459.74 2710.3,-2456.24 2710.3,-2463.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2600.49\" y=\"-2463.54\">Ry(-90°)</text></g><!-- laser_tilt_link --><g class=\"node\" id=\"node44\"><title>laser_tilt_link</title><ellipse cx=\"1674.78\" cy=\"-1974.74\" fill=\"none\" rx=\"59.6\" ry=\"59.6\" stroke=\"blue\"/><ellipse cx=\"1674.78\" cy=\"-1974.74\" fill=\"none\" rx=\"63.59\" ry=\"63.59\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1674.78\" y=\"-1971.04\">laser_tilt_link</text></g><!-- laser_tilt_mount_link&#45;&gt;laser_tilt_link --><g class=\"edge\" id=\"edge43\"><title>laser_tilt_mount_link-&gt;laser_tilt_link</title><path d=\"M1422.11,-1974.74C1477.27,-1974.74 1548.08,-1974.74 1600.53,-1974.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1600.76,-1978.24 1610.76,-1974.74 1600.76,-1971.24 1600.76,-1978.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1520.54\" y=\"-1978.54\">tz(0.03)</text></g><!-- r_shoulder_lift_link --><g class=\"node\" id=\"node46\"><title>r_shoulder_lift_link</title><ellipse cx=\"1674.78\" cy=\"-1786.74\" fill=\"none\" rx=\"81.49\" ry=\"81.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1674.78\" y=\"-1783.04\">r_shoulder_lift_link</text></g><!-- r_shoulder_pan_link&#45;&gt;r_shoulder_lift_link --><g class=\"edge\" id=\"edge45\"><title>r_shoulder_pan_link-&gt;r_shoulder_lift_link</title><path d=\"M1418.2,-1786.74C1468.09,-1786.74 1531.82,-1786.74 1583.17,-1786.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1583.26,-1790.24 1593.26,-1786.74 1583.26,-1783.24 1583.26,-1790.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1520.54\" y=\"-1790.54\">tx(0.1) * Ry(q17)</text></g><!-- r_upper_arm_roll_link --><g class=\"node\" id=\"node47\"><title>r_upper_arm_roll_link</title><ellipse cx=\"2011.02\" cy=\"-1786.74\" fill=\"none\" rx=\"90.98\" ry=\"90.98\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2011.02\" y=\"-1783.04\">r_upper_arm_roll_link</text></g><!-- r_shoulder_lift_link&#45;&gt;r_upper_arm_roll_link --><g class=\"edge\" id=\"edge46\"><title>r_shoulder_lift_link-&gt;r_upper_arm_roll_link</title><path d=\"M1756.27,-1786.74C1802.44,-1786.74 1860.81,-1786.74 1909.92,-1786.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1909.93,-1790.24 1919.93,-1786.74 1909.93,-1783.24 1909.93,-1790.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1838.03\" y=\"-1790.54\">Rx(q18)</text></g><!-- r_upper_arm_link --><g class=\"node\" id=\"node48\"><title>r_upper_arm_link</title><ellipse cx=\"2366.75\" cy=\"-1786.74\" fill=\"none\" rx=\"74.99\" ry=\"74.99\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2366.75\" y=\"-1783.04\">r_upper_arm_link</text></g><!-- r_upper_arm_roll_link&#45;&gt;r_upper_arm_link --><g class=\"edge\" id=\"edge47\"><title>r_upper_arm_roll_link-&gt;r_upper_arm_link</title><path d=\"M2102.2,-1786.74C2157.39,-1786.74 2227.54,-1786.74 2281.59,-1786.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2281.83,-1790.24 2291.83,-1786.74 2281.83,-1783.24 2281.83,-1790.24\" stroke=\"black\"/></g><!-- r_elbow_flex_link --><g class=\"node\" id=\"node49\"><title>r_elbow_flex_link</title><ellipse cx=\"2860.33\" cy=\"-1786.74\" fill=\"none\" rx=\"75.29\" ry=\"75.29\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2860.33\" y=\"-1783.04\">r_elbow_flex_link</text></g><!-- r_upper_arm_link&#45;&gt;r_elbow_flex_link --><g class=\"edge\" id=\"edge48\"><title>r_upper_arm_link-&gt;r_elbow_flex_link</title><path d=\"M2441.61,-1786.74C2530.96,-1786.74 2681.04,-1786.74 2774.89,-1786.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2774.92,-1790.24 2784.92,-1786.74 2774.92,-1783.24 2774.92,-1790.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2600.49\" y=\"-1790.54\">tx(0.4) * Ry(q19)</text></g><!-- r_forearm_roll_link --><g class=\"node\" id=\"node50\"><title>r_forearm_roll_link</title><ellipse cx=\"3314.71\" cy=\"-1786.74\" fill=\"none\" rx=\"81.49\" ry=\"81.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3314.71\" y=\"-1783.04\">r_forearm_roll_link</text></g><!-- r_elbow_flex_link&#45;&gt;r_forearm_roll_link --><g class=\"edge\" id=\"edge49\"><title>r_elbow_flex_link-&gt;r_forearm_roll_link</title><path d=\"M2935.78,-1786.74C3014.59,-1786.74 3139.01,-1786.74 3223.38,-1786.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3223.46,-1790.24 3233.46,-1786.74 3223.46,-1783.24 3223.46,-1790.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3107.67\" y=\"-1790.54\">Rx(q20)</text></g><!-- r_forearm_link --><g class=\"node\" id=\"node51\"><title>r_forearm_link</title><ellipse cx=\"3855.09\" cy=\"-1877.74\" fill=\"none\" rx=\"64.19\" ry=\"64.19\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-1874.04\">r_forearm_link</text></g><!-- r_forearm_roll_link&#45;&gt;r_forearm_link --><g class=\"edge\" id=\"edge50\"><title>r_forearm_roll_link-&gt;r_forearm_link</title><path d=\"M3394.86,-1800.12C3498.99,-1817.72 3680.55,-1848.41 3781.37,-1865.45\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3781.03,-1868.94 3791.47,-1867.16 3782.19,-1862.04 3781.03,-1868.94\" stroke=\"black\"/></g><!-- r_forearm_cam_frame --><g class=\"node\" id=\"node65\"><title>r_forearm_cam_frame</title><ellipse cx=\"3855.09\" cy=\"-1694.74\" fill=\"none\" rx=\"90.98\" ry=\"90.98\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-1691.04\">r_forearm_cam_frame</text></g><!-- r_forearm_roll_link&#45;&gt;r_forearm_cam_frame --><g class=\"edge\" id=\"edge64\"><title>r_forearm_roll_link-&gt;r_forearm_cam_frame</title><path d=\"M3394.86,-1773.21C3490.47,-1756.88 3651.35,-1729.38 3755.08,-1711.66\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3755.98,-1715.05 3765.25,-1709.92 3754.81,-1708.15 3755.98,-1715.05\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3560.25\" y=\"-1768.54\">tx(0.135) * tz(0.044) * Ry(-32.25°) * Rx(90°)</text></g><!-- r_wrist_flex_link --><g class=\"node\" id=\"node52\"><title>r_wrist_flex_link</title><ellipse cx=\"4330.71\" cy=\"-1888.74\" fill=\"none\" rx=\"71.49\" ry=\"71.49\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-1885.04\">r_wrist_flex_link</text></g><!-- r_forearm_link&#45;&gt;r_wrist_flex_link --><g class=\"edge\" id=\"edge51\"><title>r_forearm_link-&gt;r_wrist_flex_link</title><path d=\"M3919.47,-1879.21C4004.51,-1881.19 4155.58,-1884.7 4248.76,-1886.86\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4248.91,-1890.36 4258.98,-1887.1 4249.07,-1883.37 4248.91,-1890.36\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-1888.54\">tx(0.321) * Ry(q21)</text></g><!-- r_wrist_roll_link --><g class=\"node\" id=\"node53\"><title>r_wrist_roll_link</title><ellipse cx=\"4838.93\" cy=\"-1888.74\" fill=\"none\" rx=\"70.39\" ry=\"70.39\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4838.93\" y=\"-1885.04\">r_wrist_roll_link</text></g><!-- r_wrist_flex_link&#45;&gt;r_wrist_roll_link --><g class=\"edge\" id=\"edge52\"><title>r_wrist_flex_link-&gt;r_wrist_roll_link</title><path d=\"M4402.36,-1888.74C4495.98,-1888.74 4660.66,-1888.74 4758.49,-1888.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4758.62,-1892.24 4768.62,-1888.74 4758.62,-1885.24 4758.62,-1892.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4584.49\" y=\"-1892.54\">Rx(q22)</text></g><!-- r_gripper_palm_link --><g class=\"node\" id=\"node54\"><title>r_gripper_palm_link</title><ellipse cx=\"5141.2\" cy=\"-1921.74\" fill=\"none\" rx=\"83.69\" ry=\"83.69\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5141.2\" y=\"-1918.04\">r_gripper_palm_link</text></g><!-- r_wrist_roll_link&#45;&gt;r_gripper_palm_link --><g class=\"edge\" id=\"edge53\"><title>r_wrist_roll_link-&gt;r_gripper_palm_link</title><path d=\"M4908.82,-1896.31C4950.03,-1900.84 5002.92,-1906.65 5047.73,-1911.58\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5047.43,-1915.07 5057.75,-1912.68 5048.2,-1908.11 5047.43,-1915.07\" stroke=\"black\"/></g><!-- r_gripper_led_frame --><g class=\"node\" id=\"node55\"><title>r_gripper_led_frame</title><ellipse cx=\"5596.73\" cy=\"-2648.74\" fill=\"none\" rx=\"83.69\" ry=\"83.69\" stroke=\"blue\"/><ellipse cx=\"5596.73\" cy=\"-2648.74\" fill=\"none\" rx=\"87.69\" ry=\"87.69\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-2645.04\">r_gripper_led_frame</text></g><!-- r_gripper_palm_link&#45;&gt;r_gripper_led_frame --><g class=\"edge\" id=\"edge54\"><title>r_gripper_palm_link-&gt;r_gripper_led_frame</title><path d=\"M5165.08,-2002.32C5205.03,-2131.29 5298.18,-2386.38 5451.05,-2551.74 5468.26,-2570.36 5489.73,-2587.2 5510.81,-2601.41\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5508.9,-2604.34 5519.17,-2606.93 5512.76,-2598.5 5508.9,-2604.34\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-2526.54\">tx(0.0513) * tz(0.0244)</text></g><!-- r_gripper_motor_accelerometer_link --><g class=\"node\" id=\"node56\"><title>r_gripper_motor_accelerometer_link</title><ellipse cx=\"5596.73\" cy=\"-2396.74\" fill=\"none\" rx=\"141.87\" ry=\"141.87\" stroke=\"blue\"/><ellipse cx=\"5596.73\" cy=\"-2396.74\" fill=\"none\" rx=\"145.88\" ry=\"145.88\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-2393.04\">r_gripper_motor_accelerometer_link</text></g><!-- r_gripper_palm_link&#45;&gt;r_gripper_motor_accelerometer_link --><g class=\"edge\" id=\"edge55\"><title>r_gripper_palm_link-&gt;r_gripper_motor_accelerometer_link</title><path d=\"M5183.66,-1994.4C5200.6,-2021.65 5221.32,-2052.19 5243.05,-2077.74 5312.58,-2159.51 5402.45,-2240.51 5474.07,-2300.42\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5471.9,-2303.17 5481.83,-2306.88 5476.39,-2297.79 5471.9,-2303.17\" stroke=\"black\"/></g><!-- r_gripper_tool_frame --><g class=\"node\" id=\"node57\"><title>r_gripper_tool_frame</title><ellipse cx=\"5596.73\" cy=\"-2141.74\" fill=\"none\" rx=\"87.18\" ry=\"87.18\" stroke=\"blue\"/><ellipse cx=\"5596.73\" cy=\"-2141.74\" fill=\"none\" rx=\"91.18\" ry=\"91.18\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-2138.04\">r_gripper_tool_frame</text></g><!-- r_gripper_palm_link&#45;&gt;r_gripper_tool_frame --><g class=\"edge\" id=\"edge56\"><title>r_gripper_palm_link-&gt;r_gripper_tool_frame</title><path d=\"M5216.84,-1957.95C5295.93,-1996.31 5420.84,-2056.9 5505.43,-2097.93\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5503.98,-2101.12 5514.51,-2102.34 5507.04,-2094.82 5503.98,-2101.12\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-2062.54\">tx(0.18)</text></g><!-- r_gripper_motor_slider_link --><g class=\"node\" id=\"node58\"><title>r_gripper_motor_slider_link</title><ellipse cx=\"5596.73\" cy=\"-1921.74\" fill=\"none\" rx=\"111.28\" ry=\"111.28\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-1918.04\">r_gripper_motor_slider_link</text></g><!-- r_gripper_palm_link&#45;&gt;r_gripper_motor_slider_link --><g class=\"edge\" id=\"edge57\"><title>r_gripper_palm_link-&gt;r_gripper_motor_slider_link</title><path d=\"M5225.12,-1921.74C5295.23,-1921.74 5396.58,-1921.74 5475.57,-1921.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5475.58,-1925.24 5485.58,-1921.74 5475.58,-1918.24 5475.58,-1925.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-1925.54\">tx(0.16828) * tx(q23)</text></g><!-- r_gripper_l_finger_link --><g class=\"node\" id=\"node60\"><title>r_gripper_l_finger_link</title><ellipse cx=\"5596.73\" cy=\"-1699.74\" fill=\"none\" rx=\"92.88\" ry=\"92.88\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-1696.04\">r_gripper_l_finger_link</text></g><!-- r_gripper_palm_link&#45;&gt;r_gripper_l_finger_link --><g class=\"edge\" id=\"edge59\"><title>r_gripper_palm_link-&gt;r_gripper_l_finger_link</title><path d=\"M5216.84,-1885.21C5295.45,-1846.73 5419.31,-1786.1 5503.86,-1744.71\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5505.5,-1747.8 5512.95,-1740.26 5502.43,-1741.52 5505.5,-1747.8\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-1872.54\">tx(0.07691) * ty(0.01) * Rz(q25)</text></g><!-- r_gripper_r_finger_link --><g class=\"node\" id=\"node62\"><title>r_gripper_r_finger_link</title><ellipse cx=\"5596.73\" cy=\"-1494.74\" fill=\"none\" rx=\"93.68\" ry=\"93.68\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-1491.04\">r_gripper_r_finger_link</text></g><!-- r_gripper_palm_link&#45;&gt;r_gripper_r_finger_link --><g class=\"edge\" id=\"edge61\"><title>r_gripper_palm_link-&gt;r_gripper_r_finger_link</title><path d=\"M5182.67,-1848.82C5199.57,-1821.39 5220.51,-1790.79 5243.05,-1765.74 5321.74,-1678.27 5431.71,-1598.91 5507.84,-1549\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5510.02,-1551.76 5516.49,-1543.37 5506.2,-1545.89 5510.02,-1551.76\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-1769.54\">tx(0.07691) * ty(-0.01) * Rz(-q27)</text></g><!-- r_gripper_motor_screw_link --><g class=\"node\" id=\"node59\"><title>r_gripper_motor_screw_link</title><ellipse cx=\"6099.86\" cy=\"-1944.74\" fill=\"none\" rx=\"112.38\" ry=\"112.38\" stroke=\"blue\"/><ellipse cx=\"6099.86\" cy=\"-1944.74\" fill=\"none\" rx=\"116.38\" ry=\"116.38\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6099.86\" y=\"-1941.04\">r_gripper_motor_screw_link</text></g><!-- r_gripper_motor_slider_link&#45;&gt;r_gripper_motor_screw_link --><g class=\"edge\" id=\"edge58\"><title>r_gripper_motor_slider_link-&gt;r_gripper_motor_screw_link</title><path d=\"M5708.02,-1926.8C5786.23,-1930.39 5891.34,-1935.22 5972.86,-1938.96\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5973.03,-1942.47 5983.18,-1939.43 5973.36,-1935.48 5973.03,-1942.47\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5862.92\" y=\"-1942.54\">Ry(q24)</text></g><!-- r_gripper_l_finger_tip_link --><g class=\"node\" id=\"node61\"><title>r_gripper_l_finger_tip_link</title><ellipse cx=\"6099.86\" cy=\"-1699.74\" fill=\"none\" rx=\"106.68\" ry=\"106.68\" stroke=\"blue\"/><ellipse cx=\"6099.86\" cy=\"-1699.74\" fill=\"none\" rx=\"110.68\" ry=\"110.68\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6099.86\" y=\"-1696.04\">r_gripper_l_finger_tip_link</text></g><!-- r_gripper_l_finger_link&#45;&gt;r_gripper_l_finger_tip_link --><g class=\"edge\" id=\"edge60\"><title>r_gripper_l_finger_link-&gt;r_gripper_l_finger_tip_link</title><path d=\"M5689.94,-1699.74C5771.19,-1699.74 5890.08,-1699.74 5978.75,-1699.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5978.87,-1703.24 5988.87,-1699.74 5978.87,-1696.24 5978.87,-1703.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5862.92\" y=\"-1703.54\">tx(0.09137) * ty(0.00495) * Rz(-q26)</text></g><!-- r_gripper_r_finger_tip_link --><g class=\"node\" id=\"node63\"><title>r_gripper_r_finger_tip_link</title><ellipse cx=\"6099.86\" cy=\"-1463.74\" fill=\"none\" rx=\"107.48\" ry=\"107.48\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6099.86\" y=\"-1460.04\">r_gripper_r_finger_tip_link</text></g><!-- r_gripper_r_finger_link&#45;&gt;r_gripper_r_finger_tip_link --><g class=\"edge\" id=\"edge62\"><title>r_gripper_r_finger_link-&gt;r_gripper_r_finger_tip_link</title><path d=\"M5690.23,-1489.02C5772.61,-1483.92 5893.51,-1476.44 5982.44,-1470.94\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5982.81,-1474.43 5992.57,-1470.31 5982.38,-1467.44 5982.81,-1474.43\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5862.92\" y=\"-1488.54\">tx(0.09137) * ty(-0.00495) * Rz(q28)</text></g><!-- r_gripper_l_finger_tip_frame --><g class=\"node\" id=\"node64\"><title>r_gripper_l_finger_tip_frame</title><ellipse cx=\"6410.69\" cy=\"-1463.74\" fill=\"none\" rx=\"114.28\" ry=\"114.28\" stroke=\"blue\"/><ellipse cx=\"6410.69\" cy=\"-1463.74\" fill=\"none\" rx=\"118.28\" ry=\"118.28\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6410.69\" y=\"-1460.04\">r_gripper_l_finger_tip_frame</text></g><!-- r_gripper_r_finger_tip_link&#45;&gt;r_gripper_l_finger_tip_frame --><g class=\"edge\" id=\"edge63\"><title>r_gripper_r_finger_tip_link-&gt;r_gripper_l_finger_tip_frame</title><path d=\"M6207.14,-1463.74C6231.22,-1463.74 6257.03,-1463.74 6281.89,-1463.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"6282.11,-1467.24 6292.11,-1463.74 6282.11,-1460.24 6282.11,-1467.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6254.3\" y=\"-1467.54\">ty(q29)</text></g><!-- r_forearm_cam_optical_frame --><g class=\"node\" id=\"node66\"><title>r_forearm_cam_optical_frame</title><ellipse cx=\"4330.71\" cy=\"-1675.74\" fill=\"none\" rx=\"119.68\" ry=\"119.68\" stroke=\"blue\"/><ellipse cx=\"4330.71\" cy=\"-1675.74\" fill=\"none\" rx=\"123.68\" ry=\"123.68\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-1672.04\">r_forearm_cam_optical_frame</text></g><!-- r_forearm_cam_frame&#45;&gt;r_forearm_cam_optical_frame --><g class=\"edge\" id=\"edge65\"><title>r_forearm_cam_frame-&gt;r_forearm_cam_optical_frame</title><path d=\"M3946.29,-1691.12C4017.2,-1688.28 4117.02,-1684.27 4197.03,-1681.06\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4197.34,-1684.55 4207.19,-1680.65 4197.06,-1677.56 4197.34,-1684.55\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-1692.54\">Rz(-90°) * Rx(-90°)</text></g><!-- l_shoulder_lift_link --><g class=\"node\" id=\"node68\"><title>l_shoulder_lift_link</title><ellipse cx=\"1674.78\" cy=\"-1347.74\" fill=\"none\" rx=\"80.69\" ry=\"80.69\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1674.78\" y=\"-1344.04\">l_shoulder_lift_link</text></g><!-- l_shoulder_pan_link&#45;&gt;l_shoulder_lift_link --><g class=\"edge\" id=\"edge67\"><title>l_shoulder_pan_link-&gt;l_shoulder_lift_link</title><path d=\"M1418.2,-1350.01C1468.36,-1349.56 1532.53,-1348.99 1584.01,-1348.54\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1584.17,-1352.04 1594.13,-1348.45 1584.1,-1345.04 1584.17,-1352.04\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1520.54\" y=\"-1353.54\">tx(0.1) * Ry(q31)</text></g><!-- l_upper_arm_roll_link --><g class=\"node\" id=\"node69\"><title>l_upper_arm_roll_link</title><ellipse cx=\"2011.02\" cy=\"-1332.74\" fill=\"none\" rx=\"90.98\" ry=\"90.98\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2011.02\" y=\"-1329.04\">l_upper_arm_roll_link</text></g><!-- l_shoulder_lift_link&#45;&gt;l_upper_arm_roll_link --><g class=\"edge\" id=\"edge68\"><title>l_shoulder_lift_link-&gt;l_upper_arm_roll_link</title><path d=\"M1755.42,-1344.17C1801.75,-1342.09 1860.58,-1339.45 1910.02,-1337.23\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"1910.26,-1340.72 1920.09,-1336.78 1909.94,-1333.73 1910.26,-1340.72\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1838.03\" y=\"-1347.54\">Rx(q32)</text></g><!-- l_upper_arm_link --><g class=\"node\" id=\"node70\"><title>l_upper_arm_link</title><ellipse cx=\"2366.75\" cy=\"-1321.74\" fill=\"none\" rx=\"74.19\" ry=\"74.19\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2366.75\" y=\"-1318.04\">l_upper_arm_link</text></g><!-- l_upper_arm_roll_link&#45;&gt;l_upper_arm_link --><g class=\"edge\" id=\"edge69\"><title>l_upper_arm_roll_link-&gt;l_upper_arm_link</title><path d=\"M2102.2,-1329.94C2157.67,-1328.21 2228.27,-1326.02 2282.43,-1324.33\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2282.81,-1327.82 2292.69,-1324.01 2282.59,-1320.82 2282.81,-1327.82\" stroke=\"black\"/></g><!-- l_elbow_flex_link --><g class=\"node\" id=\"node71\"><title>l_elbow_flex_link</title><ellipse cx=\"2860.33\" cy=\"-1279.74\" fill=\"none\" rx=\"74.99\" ry=\"74.99\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2860.33\" y=\"-1276.04\">l_elbow_flex_link</text></g><!-- l_upper_arm_link&#45;&gt;l_elbow_flex_link --><g class=\"edge\" id=\"edge70\"><title>l_upper_arm_link-&gt;l_elbow_flex_link</title><path d=\"M2440.81,-1315.5C2530.23,-1307.86 2681.32,-1294.95 2775.41,-1286.91\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"2775.8,-1290.39 2785.46,-1286.05 2775.2,-1283.42 2775.8,-1290.39\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"2600.49\" y=\"-1314.54\">tx(0.4) * Ry(q33)</text></g><!-- l_forearm_roll_link --><g class=\"node\" id=\"node72\"><title>l_forearm_roll_link</title><ellipse cx=\"3314.71\" cy=\"-1111.74\" fill=\"none\" rx=\"80.69\" ry=\"80.69\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3314.71\" y=\"-1108.04\">l_forearm_roll_link</text></g><!-- l_elbow_flex_link&#45;&gt;l_forearm_roll_link --><g class=\"edge\" id=\"edge71\"><title>l_elbow_flex_link-&gt;l_forearm_roll_link</title><path d=\"M2930.53,-1254.04C3010.81,-1224.23 3143.14,-1175.08 3229.37,-1143.06\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3230.71,-1146.3 3238.87,-1139.53 3228.27,-1139.73 3230.71,-1146.3\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3107.67\" y=\"-1224.54\">Rx(q34)</text></g><!-- l_forearm_link --><g class=\"node\" id=\"node73\"><title>l_forearm_link</title><ellipse cx=\"3855.09\" cy=\"-1111.74\" fill=\"none\" rx=\"63.89\" ry=\"63.89\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-1108.04\">l_forearm_link</text></g><!-- l_forearm_roll_link&#45;&gt;l_forearm_link --><g class=\"edge\" id=\"edge72\"><title>l_forearm_roll_link-&gt;l_forearm_link</title><path d=\"M3395.44,-1111.74C3499.58,-1111.74 3680.45,-1111.74 3781.13,-1111.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3781.22,-1115.24 3791.22,-1111.74 3781.22,-1108.24 3781.22,-1115.24\" stroke=\"black\"/></g><!-- l_forearm_cam_frame --><g class=\"node\" id=\"node87\"><title>l_forearm_cam_frame</title><ellipse cx=\"3855.09\" cy=\"-919.74\" fill=\"none\" rx=\"90.98\" ry=\"90.98\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3855.09\" y=\"-916.04\">l_forearm_cam_frame</text></g><!-- l_forearm_roll_link&#45;&gt;l_forearm_cam_frame --><g class=\"edge\" id=\"edge86\"><title>l_forearm_roll_link-&gt;l_forearm_cam_frame</title><path d=\"M3390.85,-1084.94C3487.17,-1050.59 3654.23,-991.01 3759.22,-953.57\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"3760.62,-956.79 3768.87,-950.13 3758.27,-950.19 3760.62,-956.79\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"3560.25\" y=\"-1069.54\">tx(0.135) * tz(0.044) * Ry(-32.25°) * Rx(-90°)</text></g><!-- l_wrist_flex_link --><g class=\"node\" id=\"node74\"><title>l_wrist_flex_link</title><ellipse cx=\"4330.71\" cy=\"-1111.74\" fill=\"none\" rx=\"70.69\" ry=\"70.69\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-1108.04\">l_wrist_flex_link</text></g><!-- l_forearm_link&#45;&gt;l_wrist_flex_link --><g class=\"edge\" id=\"edge73\"><title>l_forearm_link-&gt;l_wrist_flex_link</title><path d=\"M3918.98,-1111.74C4004.11,-1111.74 4156.07,-1111.74 4249.37,-1111.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4249.61,-1115.24 4259.61,-1111.74 4249.61,-1108.24 4249.61,-1115.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-1115.54\">tx(0.321) * Ry(q35)</text></g><!-- l_wrist_roll_link --><g class=\"node\" id=\"node75\"><title>l_wrist_roll_link</title><ellipse cx=\"4838.93\" cy=\"-1056.74\" fill=\"none\" rx=\"69.59\" ry=\"69.59\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4838.93\" y=\"-1053.04\">l_wrist_roll_link</text></g><!-- l_wrist_flex_link&#45;&gt;l_wrist_roll_link --><g class=\"edge\" id=\"edge74\"><title>l_wrist_flex_link-&gt;l_wrist_roll_link</title><path d=\"M4401.3,-1104.18C4495,-1094 4661.27,-1075.93 4759.37,-1065.28\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4759.95,-1068.73 4769.52,-1064.17 4759.2,-1061.77 4759.95,-1068.73\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4584.49\" y=\"-1093.54\">Rx(q36)</text></g><!-- l_gripper_palm_link --><g class=\"node\" id=\"node76\"><title>l_gripper_palm_link</title><ellipse cx=\"5141.2\" cy=\"-792.74\" fill=\"none\" rx=\"83.39\" ry=\"83.39\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5141.2\" y=\"-789.04\">l_gripper_palm_link</text></g><!-- l_wrist_roll_link&#45;&gt;l_gripper_palm_link --><g class=\"edge\" id=\"edge75\"><title>l_wrist_roll_link-&gt;l_gripper_palm_link</title><path d=\"M4891.89,-1011.06C4941.35,-967.57 5016.11,-901.84 5070.68,-853.86\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5073.05,-856.44 5078.25,-847.21 5068.42,-851.19 5073.05,-856.44\" stroke=\"black\"/></g><!-- l_gripper_led_frame --><g class=\"node\" id=\"node77\"><title>l_gripper_led_frame</title><ellipse cx=\"5596.73\" cy=\"-1295.74\" fill=\"none\" rx=\"83.38\" ry=\"83.38\" stroke=\"blue\"/><ellipse cx=\"5596.73\" cy=\"-1295.74\" fill=\"none\" rx=\"87.39\" ry=\"87.39\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-1292.04\">l_gripper_led_frame</text></g><!-- l_gripper_palm_link&#45;&gt;l_gripper_led_frame --><g class=\"edge\" id=\"edge76\"><title>l_gripper_palm_link-&gt;l_gripper_led_frame</title><path d=\"M5185.4,-863.6C5240.1,-949.64 5340.61,-1096.09 5451.05,-1199.74 5469.4,-1216.97 5491.17,-1233.17 5512.18,-1247.18\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5510.53,-1250.28 5520.81,-1252.84 5514.37,-1244.43 5510.53,-1250.28\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-1184.54\">tx(0.0513) * tz(0.0244)</text></g><!-- l_gripper_motor_accelerometer_link --><g class=\"node\" id=\"node78\"><title>l_gripper_motor_accelerometer_link</title><ellipse cx=\"5596.73\" cy=\"-1045.74\" fill=\"none\" rx=\"141.07\" ry=\"141.07\" stroke=\"blue\"/><ellipse cx=\"5596.73\" cy=\"-1045.74\" fill=\"none\" rx=\"145.07\" ry=\"145.07\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-1042.04\">l_gripper_motor_accelerometer_link</text></g><!-- l_gripper_palm_link&#45;&gt;l_gripper_motor_accelerometer_link --><g class=\"edge\" id=\"edge77\"><title>l_gripper_palm_link-&gt;l_gripper_motor_accelerometer_link</title><path d=\"M5214.32,-832.97C5280.39,-869.83 5379.92,-925.35 5460.74,-970.43\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5459.06,-973.5 5469.5,-975.32 5462.47,-967.39 5459.06,-973.5\" stroke=\"black\"/></g><!-- l_gripper_tool_frame --><g class=\"node\" id=\"node79\"><title>l_gripper_tool_frame</title><ellipse cx=\"5596.73\" cy=\"-792.74\" fill=\"none\" rx=\"86.39\" ry=\"86.39\" stroke=\"blue\"/><ellipse cx=\"5596.73\" cy=\"-792.74\" fill=\"none\" rx=\"90.38\" ry=\"90.38\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-789.04\">l_gripper_tool_frame</text></g><!-- l_gripper_palm_link&#45;&gt;l_gripper_tool_frame --><g class=\"edge\" id=\"edge78\"><title>l_gripper_palm_link-&gt;l_gripper_tool_frame</title><path d=\"M5224.6,-792.74C5300.89,-792.74 5414.49,-792.74 5495.9,-792.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5496.16,-796.24 5506.16,-792.74 5496.16,-789.24 5496.16,-796.24\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-796.54\">tx(0.18)</text></g><!-- l_gripper_motor_slider_link --><g class=\"node\" id=\"node80\"><title>l_gripper_motor_slider_link</title><ellipse cx=\"5596.73\" cy=\"-573.74\" fill=\"none\" rx=\"110.48\" ry=\"110.48\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-570.04\">l_gripper_motor_slider_link</text></g><!-- l_gripper_palm_link&#45;&gt;l_gripper_motor_slider_link --><g class=\"edge\" id=\"edge79\"><title>l_gripper_palm_link-&gt;l_gripper_motor_slider_link</title><path d=\"M5216.33,-756.94C5290.12,-721.31 5404.15,-666.25 5487.94,-625.79\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5489.5,-628.93 5496.98,-621.43 5486.45,-622.62 5489.5,-628.93\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-744.54\">tx(0.16828) * tx(q37)</text></g><!-- l_gripper_l_finger_link --><g class=\"node\" id=\"node82\"><title>l_gripper_l_finger_link</title><ellipse cx=\"5596.73\" cy=\"-352.74\" fill=\"none\" rx=\"92.88\" ry=\"92.88\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-349.04\">l_gripper_l_finger_link</text></g><!-- l_gripper_palm_link&#45;&gt;l_gripper_l_finger_link --><g class=\"edge\" id=\"edge81\"><title>l_gripper_palm_link-&gt;l_gripper_l_finger_link</title><path d=\"M5183.64,-720.92C5200.58,-694.01 5221.3,-663.88 5243.05,-638.74 5322.38,-547.03 5433.71,-462.27 5509.94,-409.22\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5512.37,-411.8 5518.6,-403.23 5508.38,-406.05 5512.37,-411.8\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-642.54\">tx(0.07691) * ty(0.01) * Rz(q39)</text></g><!-- l_gripper_r_finger_link --><g class=\"node\" id=\"node84\"><title>l_gripper_r_finger_link</title><ellipse cx=\"5596.73\" cy=\"-140.74\" fill=\"none\" rx=\"92.88\" ry=\"92.88\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5596.73\" y=\"-137.04\">l_gripper_r_finger_link</text></g><!-- l_gripper_palm_link&#45;&gt;l_gripper_r_finger_link --><g class=\"edge\" id=\"edge83\"><title>l_gripper_palm_link-&gt;l_gripper_r_finger_link</title><path d=\"M5154.59,-710.62C5168.12,-638.85 5194.63,-533.89 5243.05,-453.74 5309.61,-343.53 5427.5,-250.6 5508.52,-195.32\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5510.91,-197.93 5517.23,-189.43 5506.99,-192.13 5510.91,-197.93\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5338.05\" y=\"-457.54\">tx(0.07691) * ty(-0.01) * Rz(-q41)</text></g><!-- l_gripper_motor_screw_link --><g class=\"node\" id=\"node81\"><title>l_gripper_motor_screw_link</title><ellipse cx=\"6099.86\" cy=\"-596.74\" fill=\"none\" rx=\"111.59\" ry=\"111.59\" stroke=\"blue\"/><ellipse cx=\"6099.86\" cy=\"-596.74\" fill=\"none\" rx=\"115.58\" ry=\"115.58\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6099.86\" y=\"-593.04\">l_gripper_motor_screw_link</text></g><!-- l_gripper_motor_slider_link&#45;&gt;l_gripper_motor_screw_link --><g class=\"edge\" id=\"edge80\"><title>l_gripper_motor_slider_link-&gt;l_gripper_motor_screw_link</title><path d=\"M5707.4,-578.77C5785.93,-582.38 5891.82,-587.24 5973.67,-590.99\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5973.63,-594.5 5983.78,-591.46 5973.95,-587.5 5973.63,-594.5\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5862.92\" y=\"-594.54\">Ry(q38)</text></g><!-- l_gripper_l_finger_tip_link --><g class=\"node\" id=\"node83\"><title>l_gripper_l_finger_tip_link</title><ellipse cx=\"6099.86\" cy=\"-352.74\" fill=\"none\" rx=\"106.68\" ry=\"106.68\" stroke=\"blue\"/><ellipse cx=\"6099.86\" cy=\"-352.74\" fill=\"none\" rx=\"110.68\" ry=\"110.68\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6099.86\" y=\"-349.04\">l_gripper_l_finger_tip_link</text></g><!-- l_gripper_l_finger_link&#45;&gt;l_gripper_l_finger_tip_link --><g class=\"edge\" id=\"edge82\"><title>l_gripper_l_finger_link-&gt;l_gripper_l_finger_tip_link</title><path d=\"M5689.94,-352.74C5771.19,-352.74 5890.08,-352.74 5978.75,-352.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5978.87,-356.24 5988.87,-352.74 5978.87,-349.24 5978.87,-356.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5862.92\" y=\"-356.54\">tx(0.09137) * ty(0.00495) * Rz(-q40)</text></g><!-- l_gripper_r_finger_tip_link --><g class=\"node\" id=\"node85\"><title>l_gripper_r_finger_tip_link</title><ellipse cx=\"6099.86\" cy=\"-117.74\" fill=\"none\" rx=\"106.68\" ry=\"106.68\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6099.86\" y=\"-114.04\">l_gripper_r_finger_tip_link</text></g><!-- l_gripper_r_finger_link&#45;&gt;l_gripper_r_finger_tip_link --><g class=\"edge\" id=\"edge84\"><title>l_gripper_r_finger_link-&gt;l_gripper_r_finger_tip_link</title><path d=\"M5689.65,-136.52C5772.22,-132.73 5893.85,-127.15 5983.05,-123.06\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"5983.38,-126.54 5993.21,-122.59 5983.06,-119.55 5983.38,-126.54\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"5862.92\" y=\"-136.54\">tx(0.09137) * ty(-0.00495) * Rz(q42)</text></g><!-- l_gripper_l_finger_tip_frame --><g class=\"node\" id=\"node86\"><title>l_gripper_l_finger_tip_frame</title><ellipse cx=\"6410.69\" cy=\"-117.74\" fill=\"none\" rx=\"113.97\" ry=\"113.97\" stroke=\"blue\"/><ellipse cx=\"6410.69\" cy=\"-117.74\" fill=\"none\" rx=\"117.98\" ry=\"117.98\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6410.69\" y=\"-114.04\">l_gripper_l_finger_tip_frame</text></g><!-- l_gripper_r_finger_tip_link&#45;&gt;l_gripper_l_finger_tip_frame --><g class=\"edge\" id=\"edge85\"><title>l_gripper_r_finger_tip_link-&gt;l_gripper_l_finger_tip_frame</title><path d=\"M6206.71,-117.74C6231.22,-117.74 6257.55,-117.74 6282.85,-117.74\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"6282.87,-121.24 6292.87,-117.74 6282.87,-114.24 6282.87,-121.24\" stroke=\"black\"/><text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"6254.3\" y=\"-121.54\">ty(q43)</text></g><!-- l_forearm_cam_optical_frame --><g class=\"node\" id=\"node88\"><title>l_forearm_cam_optical_frame</title><ellipse cx=\"4330.71\" cy=\"-899.74\" fill=\"none\" rx=\"118.88\" ry=\"118.88\" stroke=\"blue\"/><ellipse cx=\"4330.71\" cy=\"-899.74\" fill=\"none\" rx=\"122.88\" ry=\"122.88\" stroke=\"blue\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4330.71\" y=\"-896.04\">l_forearm_cam_optical_frame</text></g><!-- l_forearm_cam_frame&#45;&gt;l_forearm_cam_optical_frame --><g class=\"edge\" id=\"edge87\"><title>l_forearm_cam_frame-&gt;l_forearm_cam_optical_frame</title><path d=\"M3946.01,-915.94C4017.02,-912.94 4117.15,-908.72 4197.32,-905.33\" fill=\"none\" stroke=\"black\"/><polygon fill=\"black\" points=\"4197.65,-908.82 4207.49,-904.9 4197.35,-901.83 4197.65,-908.82\" stroke=\"black\"/><text fill=\"blue\" font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"4076.92\" y=\"-916.54\">Rz(-90°) * Rx(-90°)</text></g></g></svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pr2 = rtb.models.URDF.PR2()\n",
    "pr2.dotfile('pr2.dot')\n",
    "!dot -Tsvg pr2.dot > pr2.svg\n",
    "SVG(filename='pr2.svg')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
